diff --git a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab index 6e2ee9f..3855f73 100755 --- a/Project/Levels/RoboticManipulation/RoboticManipulation.prefab +++ b/Project/Levels/RoboticManipulation/RoboticManipulation.prefab @@ -74,17 +74,7 @@ }, "EditorEntitySortComponent": { "$type": "EditorEntitySortComponent", - "Id": 10415323403644773154, - "Child Entity Order": [ - "Instance_[31862996159500]/ContainerEntity", - "Instance_[4776417133434]/ContainerEntity", - "Instance_[28506111443834]/ContainerEntity", - "Instance_[665169744802]/ContainerEntity", - "Instance_[21971102006665]/ContainerEntity", - "Instance_[21988281875849]/ContainerEntity", - "Instance_[28142970011017]/ContainerEntity", - "Instance_[22142900698505]/ContainerEntity" - ] + "Id": 10415323403644773154 }, "EditorInspectorComponent": { "$type": "EditorInspectorComponent", @@ -106,6 +96,80 @@ "$type": "EditorVisibilityComponent", "Id": 17635043088532716457 }, + "ROS2SpawnerEditorComponent": { + "$type": "ROS2SpawnerEditorComponent", + "Id": 17096929799984361852, + "Controller": { + "Configuration": { + "Editor entity id": "Entity_[20588120919860]", + "Spawnables": { + "apple": { + "assetId": { + "guid": "{907BD09D-7E5D-5322-82BF-7BE3242A1EF6}", + "subId": 1196145064 + }, + "assetHint": "prefabs/apple.spawnable" + }, + "blue_cube": { + "assetId": { + "guid": "{ABC61B93-E050-5329-A236-10E2F61DD52E}", + "subId": 2673158184 + }, + "assetHint": "prefabs/blue_cube.spawnable" + }, + "carrot": { + "assetId": { + "guid": "{3CAB1590-BD87-5D20-8449-55960FB963DD}", + "subId": 213700765 + }, + "assetHint": "prefabs/carrot.spawnable" + }, + "corn": { + "assetId": { + "guid": "{210B3F78-B336-5F56-B8E8-ED839F4A63D9}", + "subId": 4099978870 + }, + "assetHint": "prefabs/corn.spawnable" + }, + "green_cube": { + "assetId": { + "guid": "{BFD1C1E8-64BB-5E2D-B406-A4B03C0E02FF}", + "subId": 406036830 + }, + "assetHint": "prefabs/green_cube.spawnable" + }, + "red_cube": { + "assetId": { + "guid": "{52687F52-5090-5832-B9B5-26821B074D3B}", + "subId": 1726443591 + }, + "assetHint": "prefabs/red_cube.spawnable" + }, + "tomato": { + "assetId": { + "guid": "{0F88F7B6-2DEF-54D7-A211-BE4DC5290223}", + "subId": 3484548826 + }, + "assetHint": "prefabs/tomato.spawnable" + }, + "toy_box": { + "assetId": { + "guid": "{2AB5818A-C3FB-566F-9FB1-36E95ADFB627}", + "subId": 3956998235 + }, + "assetHint": "prefabs/toy_box.spawnable" + }, + "yellow_cube": { + "assetId": { + "guid": "{183805C8-E2D9-577E-A69A-E11F92395190}", + "subId": 3747011399 + }, + "assetHint": "prefabs/black_cube.spawnable" + } + } + } + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 6284558809503441846, @@ -1016,579 +1080,72 @@ 0.8999999761581421 ], "Rotate": [ - 0.0, - 0.0, - 180.0 - ] - } - } - } - } - }, - "Instances": { - "Instance_[21971102006665]": { - "Source": "Prefabs/red_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.792099952697754 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.1538021564483643 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113156914710999 - } - ] - }, - "Instance_[21988281875849]": { - "Source": "Prefabs/green_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.0950982570648193 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.513070821762085 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113155126571655 - } - ] - }, - "Instance_[22142900698505]": { - "Source": "Prefabs/blue_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9361906051635744 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 3.1104562282562256 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113156318664551 - } - ] - }, - "Instance_[28142970011017]": { - "Source": "Prefabs/black_cube.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.990317106246948 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.8433172702789307 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7113155126571655 - } - ] - }, - "Instance_[28506111443834]": { - "Source": "Prefabs/carrot.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9181195497512815 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.309497117996216 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.725767195224762 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[47308034925857]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[31862996159500]": { - "Source": "Prefabs/tomato.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.007845401763916 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.221667766571045 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7324118614196777 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/UniformScale", - "value": 1.2999999523162842 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[45233565721889]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[4776417133434]": { - "Source": "Prefabs/corn.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.9860502481460571 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.6697561740875244 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.731939435005188 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/0", - "value": -174.7096405029297 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/1", - "value": 89.15192413330078 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/2", - "value": -90.00092315673828 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/UniformScale", - "value": 1.7000000476837158 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[46547825714465]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[6618318911619]": { - "Source": "Assets/SceneAssets.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[1146574390643]" - } - ] - }, - "Instance_[665169744802]": { - "Source": "Prefabs/apple.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[20588120919860]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 2.1052191257476807 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.648794174194336 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7312459945678711 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/0", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/1", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Centre of mass offset/2", - "value": 0.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/0", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/4", - "value": 1.0 - }, - { - "op": "replace", - "path": "/Entities/Entity_[27267717522721]/Components/EditorRigidBodyComponent/Configuration/Inertia tensor/8", - "value": 1.0 - } - ] - }, - "Instance_[712990187248]": { - "Source": "Prefabs/PandaRobot.prefab", - "Patches": [ - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[1146574390643]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": 1.5927098989486694 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 2.658244371414185 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", - "value": 0.7025851011276245 - }, - { - "op": "replace", - "path": "/Entities/Entity_[725875089136]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", - "value": "assets/urdfimporter/1440394708_panda/link7.dae.azmodel" - }, - { - "op": "replace", - "path": "/Entities/Entity_[734465023728]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", - "value": "assets/urdfimporter/1440394708_panda/link2.dae.azmodel" - }, - { - "op": "replace", - "path": "/Entities/Entity_[738759991024]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", - "value": "assets/urdfimporter/1440394708_panda/link4.dae.azmodel" - }, - { - "op": "replace", - "path": "/Entities/Entity_[743054958320]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", - "value": "assets/urdfimporter/1440394708_panda/link5.dae.azmodel" - }, - { - "op": "remove", - "path": "/Entities/Entity_[751644892912]/Components/EditorEntitySortComponent/Child Entity Order/3" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorArticulationLinkComponent/ArticulationConfiguration/Is Limited", - "value": false - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", - "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", - "value": 1 - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", - "value": "physx/rubber.physicsmaterial" - }, - { - "op": "replace", - "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", - "value": 0.05999999865889549 - }, - { - "op": "add", - "path": "/Entities/Entity_[755939860208]/Components/EditorColliderComponent", - "value": { - "$type": "EditorColliderComponent", - "Id": 13507121597919349583, - "ColliderConfiguration": { - "CollisionLayer": { - "Index": 0 - }, - "CollisionGroupId": { - "GroupId": "{00000000-0000-0000-0000-000000000000}" - }, - "Visible": false, - "Trigger": false, - "Simulated": true, - "DummySimulated": false, - "InSceneQueries": true, - "Exclusive": true, - "Position": [ - 0.0, - -0.004999999888241291, - 0.05000000074505806 - ], - "Rotation": [ - 0.0, - 0.0, - 0.0, - 1.0 - ], - "MaterialSlots": { - "Slots": [ - { - "Name": "Entire object", - "MaterialAsset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "QueueLoad", - "assetHint": "" - } - } - ] - }, - "propertyVisibilityFlags": 255, - "ColliderTag": "", - "RestOffset": 0.0, - "ContactOffset": 0.019999999552965164 - }, - "ShapeConfiguration": { - "ShapeType": 1, - "Sphere": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Radius": 0.5 - }, - "Box": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Configuration": [ - 0.07000000029802322, - 0.009999999776482582, - 0.009999999776482582 - ] - }, - "Capsule": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "Height": 1.0, - "Radius": 0.25 - }, - "Cylinder": { - "Configuration": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "CookedData": "", - "Type": 0 - }, - "Subdivision": 16, - "Height": 1.0, - "Radius": 1.0 - }, - "PhysicsAsset": { - "Asset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "QueueLoad", - "assetHint": "" - }, - "Configuration": { - "Scale": [ - 1.0, - 1.0, - 1.0 - ], - "PhysicsAsset": { - "assetId": { - "guid": "{00000000-0000-0000-0000-000000000000}", - "subId": 0 - }, - "loadBehavior": "PreLoad", - "assetHint": "" - }, - "AssetScale": [ - 1.0, - 1.0, - 1.0 - ], - "UseMaterialsFromAsset": true, - "SubdivisionLevel": 4 - } - }, - "HasNonUniformScale": false, - "SubdivisionLevel": 4 - }, - "DebugDrawSettings": { - "LocallyEnabled": true - }, - "ComponentMode": {}, - "HasNonUniformScale": false + 0.0, + 0.0, + 180.0 + ] } + } + } + } + }, + "Instances": { + "Instance_[6618318911619]": { + "Source": "Assets/SceneAssets.prefab", + "Patches": [ + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", + "value": "../Entity_[1146574390643]" + } + ] + }, + "Instance_[712990187248]": { + "Source": "Prefabs/PandaRobot.prefab", + "Patches": [ + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", + "value": "../Entity_[1146574390643]" + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", + "value": 1.5927098989486694 + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", + "value": 2.658244371414185 + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", + "value": 0.7025851011276245 + }, + { + "op": "replace", + "path": "/Entities/Entity_[725875089136]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", + "value": "assets/urdfimporter/1440394708_panda/link7.dae.azmodel" + }, + { + "op": "replace", + "path": "/Entities/Entity_[734465023728]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", + "value": "assets/urdfimporter/1440394708_panda/link2.dae.azmodel" + }, + { + "op": "replace", + "path": "/Entities/Entity_[738759991024]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", + "value": "assets/urdfimporter/1440394708_panda/link4.dae.azmodel" + }, + { + "op": "replace", + "path": "/Entities/Entity_[743054958320]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", + "value": "assets/urdfimporter/1440394708_panda/link5.dae.azmodel" + }, + { + "op": "remove", + "path": "/Entities/Entity_[751644892912]/Components/EditorEntitySortComponent/Child Entity Order/3" }, { "op": "replace", @@ -1680,11 +1237,6 @@ "path": "/Entities/Entity_[816069402352]/Components/AZ::Render::EditorMeshComponent/Controller/Configuration/ModelAsset/assetHint", "value": "assets/urdfimporter/1440394708_panda/finger.dae.azmodel" }, - { - "op": "replace", - "path": "/Entities/Entity_[824659336944]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Frame Name", - "value": "RGBDCamera3" - }, { "op": "replace", "path": "/Entities/Entity_[820364369648]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Publish Transform", @@ -2557,8 +2109,205 @@ }, { "op": "replace", - "path": "/Entities/Entity_[807479467760]/Components/EditorArticulationLinkComponent/ArticulationConfiguration/Is Limited", - "value": false + "path": "/Entities/Entity_[747349925616]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", + "value": "panda_link2" + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/0/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/1/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/2/1", + "value": -1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/3/1", + "value": -2.450000047683716 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/4/1", + "value": 1.5709999799728394 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/6/1", + "value": -0.10000000149011612 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/7/1", + "value": 0.019999999552965164 + }, + { + "op": "replace", + "path": "/Entities/Entity_[824659336944]/Components/JointsManipulationEditorComponent/Initial positions/8/1", + "value": 0.019999999552965164 + }, + { + "op": "add", + "path": "/Entities/Entity_[755939860208]/Components/EditorColliderComponent", + "value": { + "$type": "EditorColliderComponent", + "Id": 13507121597919349583, + "ColliderConfiguration": { + "CollisionLayer": { + "Index": 0 + }, + "CollisionGroupId": { + "GroupId": "{00000000-0000-0000-0000-000000000000}" + }, + "Visible": false, + "Trigger": false, + "Simulated": true, + "DummySimulated": false, + "InSceneQueries": true, + "Exclusive": true, + "Position": [ + 0.0, + -0.004999999888241291, + 0.05000000074505806 + ], + "Rotation": [ + 0.0, + 0.0, + 0.0, + 1.0 + ], + "MaterialSlots": { + "Slots": [ + { + "Name": "Entire object", + "MaterialAsset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "QueueLoad", + "assetHint": "" + } + } + ] + }, + "propertyVisibilityFlags": 255, + "ColliderTag": "", + "RestOffset": 0.0, + "ContactOffset": 0.019999999552965164 + }, + "ShapeConfiguration": { + "ShapeType": 1, + "Sphere": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Radius": 0.5 + }, + "Box": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Configuration": [ + 0.07000000029802322, + 0.009999999776482582, + 0.009999999776482582 + ] + }, + "Capsule": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "Height": 1.0, + "Radius": 0.25 + }, + "Cylinder": { + "Configuration": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "CookedData": "", + "Type": 0 + }, + "Subdivision": 16, + "Height": 1.0, + "Radius": 1.0 + }, + "PhysicsAsset": { + "Asset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "QueueLoad", + "assetHint": "" + }, + "Configuration": { + "Scale": [ + 1.0, + 1.0, + 1.0 + ], + "PhysicsAsset": { + "assetId": { + "guid": "{00000000-0000-0000-0000-000000000000}", + "subId": 0 + }, + "loadBehavior": "PreLoad", + "assetHint": "" + }, + "AssetScale": [ + 1.0, + 1.0, + 1.0 + ], + "UseMaterialsFromAsset": true, + "SubdivisionLevel": 4 + } + }, + "HasNonUniformScale": false, + "SubdivisionLevel": 4 + }, + "DebugDrawSettings": { + "LocallyEnabled": true + }, + "ComponentMode": {}, + "HasNonUniformScale": false + } + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{6D9F8E0A-466E-5774-8974-FC7F0872FD00}" + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 1 + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "physx/rubber.physicsmaterial" + }, + { + "op": "replace", + "path": "/Entities/Entity_[755939860208]/Components/EditorMeshColliderComponent/ColliderConfiguration/ContactOffset", + "value": 0.05999999865889549 }, { "op": "add", @@ -2721,11 +2470,6 @@ "op": "replace", "path": "/Entities/Entity_[807479467760]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", "value": "panda_leftfinger" - }, - { - "op": "replace", - "path": "/Entities/Entity_[747349925616]/Components/ROS2FrameEditorComponent/ROS2FrameConfiguration/Namespace Configuration/Namespace", - "value": "panda_link2" } ] }, diff --git a/Project/Prefabs/apple.prefab b/Project/Prefabs/apple.prefab index 0e834e1..d3b041e 100755 --- a/Project/Prefabs/apple.prefab +++ b/Project/Prefabs/apple.prefab @@ -15,7 +15,7 @@ "$type": "EditorEntitySortComponent", "Id": 10306973598365841266, "Child Entity Order": [ - "Entity_[534760181612]" + "Entity_[27267717522721]" ] }, "EditorInspectorComponent": { @@ -162,60 +162,19 @@ "$type": "EditorVisibilityComponent", "Id": 17099443271266381888 }, - "TransformComponent": { - "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", - "Id": 17921041119350952090, - "Parent Entity": "Entity_[534760181612]" - } - } - }, - "Entity_[534760181612]": { - "Id": "Entity_[534760181612]", - "Name": "apple", - "Components": { - "EditorDisabledCompositionComponent": { - "$type": "EditorDisabledCompositionComponent", - "Id": 18044469881547845778 - }, - "EditorEntityIconComponent": { - "$type": "EditorEntityIconComponent", - "Id": 12292306854165822588 - }, - "EditorEntitySortComponent": { - "$type": "EditorEntitySortComponent", - "Id": 8314993489357136941, - "Child Entity Order": [ - "Entity_[27267717522721]" - ] - }, - "EditorInspectorComponent": { - "$type": "EditorInspectorComponent", - "Id": 17137814805974204638, - "ComponentOrderEntryArray": [ - { - "ComponentId": 8647185006201569915 - } - ] - }, - "EditorLockComponent": { - "$type": "EditorLockComponent", - "Id": 4475825274964774522 - }, - "EditorOnlyEntityComponent": { - "$type": "EditorOnlyEntityComponent", - "Id": 2442758587829817613 - }, - "EditorPendingCompositionComponent": { - "$type": "EditorPendingCompositionComponent", - "Id": 11225710462403311240 - }, - "EditorVisibilityComponent": { - "$type": "EditorVisibilityComponent", - "Id": 16780360982239074981 + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 9671399038384703516, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", - "Id": 8647185006201569915, + "Id": 17921041119350952090, "Parent Entity": "ContainerEntity", "Transform Data": { "UniformScale": 0.05000000074505806 diff --git a/Project/Prefabs/black_cube.prefab b/Project/Prefabs/black_cube.prefab index 0c12470..8b73b82 100755 --- a/Project/Prefabs/black_cube.prefab +++ b/Project/Prefabs/black_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[9165443789406]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/blue_cube.prefab b/Project/Prefabs/blue_cube.prefab index 2642738..28198ea 100755 --- a/Project/Prefabs/blue_cube.prefab +++ b/Project/Prefabs/blue_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[1152437262922]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/carrot.prefab b/Project/Prefabs/carrot.prefab index 4d5cf32..85ece79 100755 --- a/Project/Prefabs/carrot.prefab +++ b/Project/Prefabs/carrot.prefab @@ -379,6 +379,16 @@ "$type": "EditorVisibilityComponent", "Id": 11409618612434829098 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 10470715774139935384, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 18391610517830378569, diff --git a/Project/Prefabs/corn.prefab b/Project/Prefabs/corn.prefab index 4323b65..1740612 100755 --- a/Project/Prefabs/corn.prefab +++ b/Project/Prefabs/corn.prefab @@ -226,9 +226,9 @@ }, "Configuration": { "Scale": [ - 0.10199999809265137, - 0.10199999809265137, - 0.10199999809265137 + 0.05999999865889549, + 0.05999999865889549, + 0.05999999865889549 ], "PhysicsAsset": { "assetId": { @@ -260,20 +260,20 @@ "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.0003767325251828879, - -0.0012333319755271077, - -0.0012046174379065633 + -0.0002216073771705851, + -0.0007254894007928669, + -0.0007085984689183533 ], "Inertia tensor": [ - 0.0001341252209385857, + 0.000046410106733674183, 0.0, 0.0, 0.0, - 0.00013628810120280832, + 0.000047158508095890284, 0.0, 0.0, 0.0, - 0.00003456683407421224 + 0.00001196084213006543 ] } }, @@ -281,6 +281,16 @@ "$type": "EditorVisibilityComponent", "Id": 12802023452715689730 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 16221295209102460307, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 10754169405606624831, diff --git a/Project/Prefabs/green_cube.prefab b/Project/Prefabs/green_cube.prefab index 9ca8d53..d48039f 100755 --- a/Project/Prefabs/green_cube.prefab +++ b/Project/Prefabs/green_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[5420232307294]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/red_cube.prefab b/Project/Prefabs/red_cube.prefab index 3f4195d..a79ae4b 100755 --- a/Project/Prefabs/red_cube.prefab +++ b/Project/Prefabs/red_cube.prefab @@ -58,11 +58,6 @@ "$type": "EditorColliderComponent", "Id": 11542677749643201060, "ColliderConfiguration": { - "Position": [ - 0.0, - 0.0, - 0.02500000037252903 - ], "MaterialSlots": { "Slots": [ { @@ -266,6 +261,11 @@ "Id": 16843170302531117928, "Parent Entity": "Entity_[485314884190]", "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.02500000037252903 + ], "UniformScale": 0.05000000074505806 } } diff --git a/Project/Prefabs/tomato.prefab b/Project/Prefabs/tomato.prefab index f32ffbe..a436eda 100755 --- a/Project/Prefabs/tomato.prefab +++ b/Project/Prefabs/tomato.prefab @@ -107,9 +107,9 @@ }, "Configuration": { "Scale": [ - 0.05199999734759331, - 0.05199999734759331, - 0.05199999734759331 + 0.03999999910593033, + 0.03999999910593033, + 0.03999999910593033 ], "PhysicsAsset": { "assetId": { @@ -141,20 +141,20 @@ "Compute Mass": false, "Mass": 0.20000000298023224, "Centre of mass offset": [ - -0.00014860315422993153, - 0.00003289386222604662, - 0.0016063579823821783 + -0.00011431012535467744, + 0.00002530297206249088, + 0.001235659932717681 ], "Inertia tensor": [ - 0.00004531557351583615, + 0.000026813961085281335, 0.0, 0.0, 0.0, - 0.0000491143437102437, + 0.000029061740860925056, 0.0, 0.0, 0.0, - 0.000052105468057561666 + 0.00003083164119743742 ] } }, @@ -162,6 +162,16 @@ "$type": "EditorVisibilityComponent", "Id": 4910412103323004378 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 1369596616863894338, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 14610634284382918424, diff --git a/Project/Prefabs/toy_box.prefab b/Project/Prefabs/toy_box.prefab index c350171..903fc13 100755 --- a/Project/Prefabs/toy_box.prefab +++ b/Project/Prefabs/toy_box.prefab @@ -277,6 +277,16 @@ "$type": "EditorVisibilityComponent", "Id": 9097636690885332082 }, + "ROS2FrameEditorComponent": { + "$type": "ROS2FrameEditorComponent", + "Id": 5197297354537371193, + "ROS2FrameConfiguration": { + "Namespace Configuration": { + "Namespace Strategy": 2 + }, + "Frame Name": "" + } + }, "TransformComponent": { "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", "Id": 4486871887464016633, diff --git a/ros2_ws/src/rai_mani/package.xml b/ros2_ws/src/rai_mani/package.xml new file mode 100644 index 0000000..70cb158 --- /dev/null +++ b/ros2_ws/src/rai_mani/package.xml @@ -0,0 +1,18 @@ + + + + rai_mani + 0.0.0 + TODO: Package description + kdabrowski + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2_ws/src/rai_mani/rai_mani/__init__.py b/ros2_ws/src/rai_mani/rai_mani/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/rai_mani/rai_mani/benchmark.py b/ros2_ws/src/rai_mani/rai_mani/benchmark.py new file mode 100644 index 0000000..c4ff0d2 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/benchmark.py @@ -0,0 +1,25 @@ +import rclpy + +from rai_mani.manager import RaiBenchmarkManager + +from rai_mani.scenarios.move_to_the_left import MoveToTheLeft +from rai_mani.scenarios.place_on_top import PlaceOnTop +from rai_mani.scenarios.replace_types import ReplaceTypes +from rai_mani.scenarios.longest_object import LongestObject + +from rclpy.executors import MultiThreadedExecutor + +def main(args=None): + rclpy.init(args=args) + + manager = RaiBenchmarkManager([PlaceOnTop, LongestObject, MoveToTheLeft, ReplaceTypes], list(range(4))) + + executor = MultiThreadedExecutor(2) + executor.add_node(manager) + executor.spin() + + manager.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ros2_ws/src/rai_mani/rai_mani/manager.py b/ros2_ws/src/rai_mani/rai_mani/manager.py new file mode 100644 index 0000000..febfe40 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/manager.py @@ -0,0 +1,165 @@ +import rclpy +from rclpy.node import Node +from gazebo_msgs.srv import SpawnEntity, DeleteEntity +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion + +from tf2_ros import Buffer, TransformListener + +from rai_interfaces.srv import ManipulatorMoveTo + +from langchain_core.messages import HumanMessage + +from rai.agents.conversational_agent import create_conversational_agent +from rai.node import RaiBaseNode +from rai.tools.ros.manipulation import GetObjectPositionsTool, MoveToPointTool +from rai.tools.ros.native import GetCameraImage, Ros2GetTopicsNamesAndTypesTool +from rai.utils.model_initialization import get_llm_model + +from threading import Thread + +from rai_mani.scenarios.scenario_base import ScenarioBase + +import random +import time + +class ScenarioManager(Node): + """ + A class responsible for playing the scenarios + """ + def __init__(self, scenario_types, seeds = []): + """ + Initializes the ScenarioManager + + Args: + scenario_types: A list of scenario classes to play + seeds: A list of seeds to use for each scenario + """ + super().__init__('scenario_manager') + self.scenario_types = scenario_types + self.seeds = seeds + + self.spawn_client = self.create_client(SpawnEntity, '/spawn_entity') + self.delete_client = self.create_client(DeleteEntity, '/delete_entity') + self.manipulator_client = self.create_client(ManipulatorMoveTo, '/manipulator_move_to') + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) + + while not self.spawn_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + while not self.delete_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + + self.timer = self.create_timer(1.0, self.timer_callback) + self.scenario: ScenarioBase = None + self.current_scenario = 0 + self.agent_thread: Thread = None + self.manipulator_ready = False + self.scores = [] + + def _init_scenario(self): + self.scenario = self.scenario_types[self.current_scenario](self.spawn_client, self.delete_client, self.manipulator_client, self) + self.manipulator_ready = False + request = ManipulatorMoveTo.Request() + request.target_pose.pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + request.target_pose.pose.position = Point(x=0.2, y=0.0, z=0.2) + if self.current_scenario < len(self.seeds): + random.seed(self.seeds[self.current_scenario]) + else: + random.seed(42) + def callback(future: Future): + self.manipulator_ready = True + self.scenario.reset() + self.scenario.manipulator_client.call_async(request).add_done_callback(callback) + + def _terminate_scenario(self): + self.get_logger().info(f'Scenario terminated with score {self.scores[-1]}') + self.scenario = None + self.tf2_buffer = Buffer() + self.tf2_listener = TransformListener(self.tf2_buffer, self) + if self.current_scenario == len(self.scenario_types) - 1: + self.get_logger().info(f'All scenarios are completed, with scores: {self.scores}') + request = ManipulatorMoveTo.Request() + request.target_pose.pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + request.target_pose.pose.position = Point(x=0.2, y=0.0, z=0.2) + def callback(future: Future): + self.manipulator_ready = True + self.executor.shutdown() + self.manipulator_client.call_async(request).add_done_callback(callback) + self.timer.cancel() + return + self.current_scenario = (self.current_scenario + 1) % len(self.scenario_types) + self.manipulator_ready = False + + def timer_callback(self): + if self.scenario == None: + self._init_scenario() + + if not self.manipulator_ready: + return + + progress, terminated = self.scenario.step() + if terminated and not (self.agent_thread and self.agent_thread.is_alive()): + self.scores.append(progress) + self._terminate_scenario() + return + if self.agent_thread and not self.agent_thread.is_alive(): + self.get_logger().info('Agent failed to fulfill the task, terminating the scenario.') + self.scores.append(progress) + self._terminate_scenario() + +class RaiBenchmarkManager(ScenarioManager): + """ + A class responsible for playing the scenarios and running the conversational agent for each scenario + """ + def __init__(self, scenario_types, seeds = []): + super().__init__(scenario_types, seeds) + self.agent = None + + def _init_scenario(self): + super()._init_scenario() + + self.rai_node = RaiBaseNode(node_name="manipulation_demo") + self.rai_node.declare_parameter("conversion_ratio", 1.0) + self.rai_node.qos_profile.reliability = rclpy.qos.ReliabilityPolicy.RELIABLE + + tools = [ + GetObjectPositionsTool( + node=self.rai_node, + target_frame="panda_link0", + source_frame="RGBDCamera5", + camera_topic="/color_image5", + depth_topic="/depth_image5", + camera_info_topic="/color_camera_info5", + ), + MoveToPointTool(node=self.rai_node, manipulator_frame="panda_link0"), + GetCameraImage(node=self.rai_node), + Ros2GetTopicsNamesAndTypesTool(node=self.rai_node), + ] + + llm = get_llm_model(model_type="complex_model") + + system_prompt = """ + You are a robotic arm with interfaces to detect and manipulate objects. + Here are the coordinates information: + x - front to back (positive is forward) + y - left to right (positive is right) + z - up to down (positive is up) + + Before starting the task, make sure to grab the camera image to understand the environment. + """ + + self.agent = create_conversational_agent( + llm=llm, + tools=tools, + system_prompt=system_prompt, + ) + + def run_agent(): + time.sleep(1) + self.agent.invoke({"messages": [HumanMessage(content=self.scenario.get_prompt())]})["messages"][-1].pretty_print() + + self.agent_thread = Thread(target=run_agent) + self.agent_thread.start() + diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py new file mode 100644 index 0000000..8f477cf --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/longest_object.py @@ -0,0 +1,105 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class LongestObject(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def get_prompt(self): + return "Put the longest object from the table into the toy box." + + def reset(self): + super().reset() + + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + self.spawn_entities_in_random_positions(prefabs, prefabs) + + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=-0.5, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)).pose + self.spawn_entity('toy_box', 'toy_box', pose_transformed) + + def calculate_progress(self): + if len(self.entities) == 0: + return 0.0 + + carrot_position = self.pose_transformed(self.get_entity_pose('carrot')).position + toy_box_position = self.pose_transformed(self.get_entity_pose('toy_box')).position + + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2)**0.5 + + return max(0.0, 1.0 - 3.0 * distance(carrot_position, toy_box_position)) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.5 + +class LongestObjectAuto(LongestObject): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def place_on_top(self, bot_object: str, top_object: str): + pose = self.get_entity_pose(top_object) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose = self.get_entity_pose(bot_object) + pose.position.z += 0.2 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.place_on_top('toy_box', 'carrot') + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py new file mode 100644 index 0000000..55a15d6 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/move_to_the_left.py @@ -0,0 +1,102 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class MoveToTheLeft(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def get_prompt(self): + return "There are 5 apples on the right half of the table. Their Y position is positive. Move each of them to the left half of the table, such that they dont collide with each other. The Y position of the apples should be negative after the task is completed. First grab one apple using the 'grab' tool, and then drop it in the appropriate position using the 'drop' tool. Repeat this procedure for each apple." + + def reset(self): + super().reset() + + for i in range(5): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position = Point(x=0.4, y=float(i)/10 + 0.15, z=0.1) + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + + prefab = 'apple' + self.spawn_entity(prefab, f'{prefab}{i}', pose_transformed.pose) + + def calculate_progress(self): + num_apples = sum(1 for name in self.entities if name.startswith('apple')) + num_good_apples = sum(1 for name in self.entities if name.startswith('apple') and self.pose_transformed(self.get_entity_pose(name)).position.y < 0.0) + return num_good_apples / num_apples + + def step(self): + if len(self.entities) == 0: # The entities are not spawned yet + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 1.0 + +class MoveToTheLeftAuto(MoveToTheLeft): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + super().__init__(spawn_client, delete_client, manipulator_client, node) + self.manipulator_busy = False + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def move_to_the_left(self, name: str): + pose = self.get_entity_pose(name) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose.position.y -= 0.6 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: # The entities are not spawned yet + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0: + for name in self.entities: + pose = self.pose_transformed(self.get_entity_pose(name)) + if pose.position.y > 0.0: + self.node.get_logger().info(f'Moving {name} to the left from {pose.position.y}') + self.move_to_the_left(name) + break + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 1.0 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py new file mode 100644 index 0000000..ac89625 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/place_on_top.py @@ -0,0 +1,106 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class PlaceOnTop(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.top_object = None + self.bot_object = None + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def get_prompt(self): + return "Place the yellow cube on top of the blue cube. Remember to increase the Z position of the 'drop' task by around 0.2 to avoid collision." + + def reset(self): + super().reset() + + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + self.spawn_entities_in_random_positions(prefabs, prefabs) + self.bot_object = 'blue_cube' + self.top_object = 'yellow_cube' + + def calculate_progress(self): + if self.top_object == None or self.bot_object == None: + return 0.0 + + top_pose = self.pose_transformed(self.get_entity_pose(self.top_object)) + bot_pose = self.pose_transformed(self.get_entity_pose(self.bot_object)) + + goal_position = bot_pose.position + goal_position.z += 0.05 + + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2 + (a.z - b.z)**2)**0.5 + + return max(0.0, 1.0 - 10.0 * distance(top_pose.position, goal_position)) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.8 + +class PlaceOnTopAuto(PlaceOnTop): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def place_on_top(self, bot_object: str, top_object: str): + pose = self.get_entity_pose(top_object) + pose.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = False + self.manipulator_queue.append(req) + + pose = self.get_entity_pose(bot_object) + pose.position.z += 0.2 + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = self.pose_transformed(pose) + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.place_on_top(self.bot_object, self.top_object) + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py new file mode 100644 index 0000000..5bcd42e --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/replace_types.py @@ -0,0 +1,170 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node +from rclpy.task import Future + +from geometry_msgs.msg import Point, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +from rai_mani.scenarios.scenario_base import ScenarioBase + +class ReplaceTypes(ScenarioBase): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.vegetable_poses = [] + self.toy_poses = [] + self.current_index = 0 + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def get_prompt(self): + return "Replace the objects in such a way that all the toys are in the places of vegetables and vice versa." + + def reset(self): + super().reset() + + self.vegetable_poses = [] + self.toy_poses = [] + self.current_index = 0 + prefabs = ['apple', 'yellow_cube', 'blue_cube', 'carrot'] + self.spawn_entities_in_random_positions(prefabs, prefabs) + for entity in prefabs: + if self._get_type(entity) == 'vegetable': + self.vegetable_poses.append(self.pose_transformed(self.get_entity_pose(entity))) + elif self._get_type(entity) == 'toy': + self.toy_poses.append(self.pose_transformed(self.get_entity_pose(entity))) + return + + def _get_type(self, object_name: str): + if object_name.startswith('apple') or object_name.startswith('carrot'): + return 'vegetable' + elif object_name.startswith('yellow_cube') or object_name.startswith('blue_cube'): + return 'toy' + else: + return None + + def _min_distance(self, entity: str): + def distance(a, b): + return ((a.x - b.x)**2 + (a.y - b.y)**2)**0.5 + pose = self.pose_transformed(self.get_entity_pose(entity)) + min_distance = float('inf') + if self._get_type(entity) == 'vegetable': + for toy_pose in self.toy_poses: + min_distance = min(min_distance, distance(pose.position, toy_pose.position)) + elif self._get_type(entity) == 'toy': + for vegetable_pose in self.vegetable_poses: + min_distance = min(min_distance, distance(pose.position, vegetable_pose.position)) + return min_distance + + def calculate_progress(self): + if self.vegetable_poses == [] or self.toy_poses == []: + return 0.0 + + progress = 0.0 + + for entity in self.entities: + progress += max(0.0, 1.0 - self._min_distance(entity) * 10.0) + + return progress / len(self.entities) + + def step(self): + if len(self.entities) == 0: + return 0.0, False + + progress = self.calculate_progress() + + return progress, progress >= 0.8 + +class ReplaceTypesAuto(ReplaceTypes): + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Client, node: Node): + self.manipulator_busy = False + + super().__init__(spawn_client, delete_client, manipulator_client, node) + + def reset(self): + super().reset() + + self.manipulator_busy = False + self.manipulator_queue = [] + + def replace(self, a: str, b: str): + from copy import deepcopy + pose_a = self.pose_transformed(self.get_entity_pose(a)) + pose_a.position.z += 0.1 + + buffer_pose = deepcopy(pose_a) + buffer_pose.position.x = 0.4 + buffer_pose.position.y = -0.5 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = pose_a + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = buffer_pose + req.final_gripper_state = True + self.manipulator_queue.append(req) + + pose_b = self.pose_transformed(self.get_entity_pose(b)) + pose_b.position.z += 0.1 + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = pose_b + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = pose_a + req.final_gripper_state = True + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = True + req.target_pose.pose = buffer_pose + req.final_gripper_state = False + self.manipulator_queue.append(req) + + req = ManipulatorMoveTo.Request() + req.initial_gripper_state = False + req.target_pose.pose = pose_b + req.final_gripper_state = True + self.manipulator_queue.append(req) + + def move_callback(self, future: Future): + result = future.result() + if result.success: + self.node.get_logger().debug(f'Move performed') + else: + self.node.get_logger().error(f'Failed to perform move') + self.manipulator_busy = False + + def step(self): + if len(self.entities) < 4: + return 0.0, False + + progress = self.calculate_progress() + + if not self.manipulator_busy: + vegetables = [] + toys = [] + for entity in self.entities: + if self._get_type(entity) == 'vegetable': + vegetables.append(entity) + elif self._get_type(entity) == 'toy': + toys.append(entity) + + if len(self.manipulator_queue) == 0 and progress < 0.8: + self.replace(vegetables[self.current_index], toys[self.current_index]) + self.current_index = (self.current_index + 1) % len(vegetables) + + if len(self.manipulator_queue) > 0: + req = self.manipulator_queue.pop(0) + self.manipulator_busy = True + self.manipulator_client.call_async(req).add_done_callback(self.move_callback) + + return progress, progress >= 0.8 and not self.manipulator_busy \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py new file mode 100644 index 0000000..d7f3775 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/scenarios/scenario_base.py @@ -0,0 +1,128 @@ +import rclpy +from rclpy.client import Client +from rclpy.node import Node + +from gazebo_msgs.srv import SpawnEntity, DeleteEntity +from rclpy.task import Future +from geometry_msgs.msg import Pose, Quaternion, PoseStamped + +from rai_interfaces.srv import ManipulatorMoveTo + +import random + +class ScenarioBase: + """ + Base class for a scenario. A scenario is a task that the agent has to perform. + """ + def __init__(self, spawn_client: Client, delete_client: Client, manipulator_client: Node, node: Node): + self.spawn_client = spawn_client + self.delete_client = delete_client + self.manipulator_client = manipulator_client + self.node = node + self.entities: dict[str, str] = {} + + def __del__(self): + for name in self.entities: + self.delete_entity(name) + + def get_prompt(self) -> str: + """ + Returns a prompt that describes the task that the agent has to perform. + """ + return "Please do something interesting" + + def reset(self) -> None: + """ + Resets the scenario to its initial state. + + This method is called before the scenario is started. + """ + + for name in self.entities: + self.delete_entity(name) + self.entities = {} + + def spawn_entity(self, prefab_name: str, name: str, pose: Pose) -> None: + """ + Spawns an entity in the simulation. + + Args: + prefab_name: The name of the prefab to spawn. + name: The name of the entity, by which it can be later referenced. + pose: The pose of the entity. + """ + req = SpawnEntity.Request() + req.name = prefab_name + req.xml = '' + req.robot_namespace = name + req.initial_pose = pose + + self.spawn_client.call_async(req).add_done_callback(lambda future: self.entity_spawned_callback(future, name)) + + def spawn_entities_in_random_positions(self, prefab_names: list[str], names: list[str]) -> None: + """ + Spawns entities randomly positione around the table. + + Args: + prefab_names: A list of prefab names to spawn. + names: A list of names for the entities, by which they can be later referenced. + """ + grid = [ + (x / 10.0 + 0.4, y / 10.0) for x in range(-1, 2) for y in range(-3, 4) + ] + + positions = random.sample(grid, k=len(prefab_names)) + for prefab_name, name, position in zip(prefab_names, names, positions): + pose = PoseStamped() + pose.header.frame_id = 'world' + pose.pose.position.x = position[0] + pose.pose.position.y = position[1] + pose.pose.position.z = 0.05 + pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + pose_transformed = self.node.tf2_buffer.transform(pose, 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + self.spawn_entity(prefab_name, name, pose_transformed.pose) + + def delete_entity(self, name: str) -> None: + req = DeleteEntity.Request() + req.name = self.entities[name] + + self.delete_client.call_async(req) + + def get_entity_pose(self, name: str) -> Pose: + """ + Returns the pose of an entity. + """ + pose = PoseStamped() + entity_frame = name + '/' + pose.header.frame_id = entity_frame + pose = self.node.tf2_buffer.transform(pose, entity_frame + 'odom', timeout=rclpy.time.Duration(seconds=5.0)) + return pose.pose + + def pose_transformed(self, pose: Pose) -> Pose: + """ + Transforms the pose into the frame of the panda robot. + """ + pose_stamped = PoseStamped() + pose_stamped.pose = pose + pose_stamped.header.frame_id = 'odom' + pose = self.node.tf2_buffer.transform(pose_stamped, 'world', timeout=rclpy.time.Duration(seconds=5.0)).pose + pose.orientation = Quaternion(x=0.923880, y=-0.382683, z=0.0, w=0.0) + return pose + + def step(self) -> tuple[float, bool]: + """ + Performs a step in the scenario. + + Returns: + progress: A float between 0.0 and 1.0 that represents the progress of the task. + terminated: A boolean that indicates whether the task is terminated. + """ + return 0.0, False + + def entity_spawned_callback(self, future: Future, name: str): + result = future.result() + if result.success: + self.node.get_logger().info(f'Entity spawned: {name} ({result.status_message})') + self.entities[name] = result.status_message + else: + self.node.get_logger().error(f'Failed to spawn entity: {result.status_message}') \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/rai_mani/showcase.py b/ros2_ws/src/rai_mani/rai_mani/showcase.py new file mode 100644 index 0000000..cafb265 --- /dev/null +++ b/ros2_ws/src/rai_mani/rai_mani/showcase.py @@ -0,0 +1,25 @@ +import rclpy + +from rai_mani.manager import ScenarioManager + +from rai_mani.scenarios.move_to_the_left import MoveToTheLeftAuto +from rai_mani.scenarios.place_on_top import PlaceOnTopAuto +from rai_mani.scenarios.replace_types import ReplaceTypesAuto +from rai_mani.scenarios.longest_object import LongestObjectAuto + +from rclpy.executors import MultiThreadedExecutor + +def main(args=None): + rclpy.init(args=args) + + manager = ScenarioManager([PlaceOnTopAuto, LongestObjectAuto, MoveToTheLeftAuto, ReplaceTypesAuto], list(range(4))) + + executor = MultiThreadedExecutor(2) + executor.add_node(manager) + executor.spin() + + manager.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/rai_mani/resource/rai_mani b/ros2_ws/src/rai_mani/resource/rai_mani new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/rai_mani/setup.cfg b/ros2_ws/src/rai_mani/setup.cfg new file mode 100644 index 0000000..74703e5 --- /dev/null +++ b/ros2_ws/src/rai_mani/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rai_mani +[install] +install_scripts=$base/lib/rai_mani diff --git a/ros2_ws/src/rai_mani/setup.py b/ros2_ws/src/rai_mani/setup.py new file mode 100644 index 0000000..ad1e520 --- /dev/null +++ b/ros2_ws/src/rai_mani/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'rai_mani' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='kdabrowski', + maintainer_email='kacper.dabrowski@robotec.ai', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'benchmark = rai_mani.benchmark:main', + 'showcase = rai_mani.showcase:main' + ], + }, +) diff --git a/ros2_ws/src/rai_mani/test/test_copyright.py b/ros2_ws/src/rai_mani/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ros2_ws/src/rai_mani/test/test_flake8.py b/ros2_ws/src/rai_mani/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ros2_ws/src/rai_mani/test/test_pep257.py b/ros2_ws/src/rai_mani/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/rai_mani/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'