From f3bcae767e6e1bf1792449cd766fd1fd4124f72d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 5 Jan 2024 13:08:48 +0100 Subject: [PATCH 01/12] [designator] refactored action_designator --- examples/custom_resolver.ipynb | 81 +-- examples/minimal_task_tree.ipynb | 282 +++++---- examples/orm_example.ipynb | 185 +++--- examples/orm_querying_examples.ipynb | 29 +- src/pycram/designator.py | 23 +- src/pycram/designators/action_designator.py | 610 ++------------------ src/pycram/designators/actions/actions.py | 578 +++++++++++++++++++ src/pycram/task.py | 10 +- test/test_action_designator.py | 12 +- test/test_database_resolver.py | 16 +- test/test_jpt_resolver.py | 14 +- test/test_task_tree.py | 8 +- 12 files changed, 1001 insertions(+), 847 deletions(-) create mode 100644 src/pycram/designators/actions/actions.py diff --git a/examples/custom_resolver.ipynb b/examples/custom_resolver.ipynb index 0660276e4..21e1bf7cc 100644 --- a/examples/custom_resolver.ipynb +++ b/examples/custom_resolver.ipynb @@ -23,8 +23,8 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:13:58.323072599Z", - "start_time": "2023-12-13T10:13:56.448691067Z" + "end_time": "2024-01-05T11:39:35.037185202Z", + "start_time": "2024-01-05T11:39:32.842006673Z" } }, "outputs": [ @@ -32,15 +32,22 @@ "name": "stdout", "output_type": "stream", "text": [ - "Requirement already up-to-date: probabilistic_model in /home/dprueser/.local/lib/python3.8/site-packages (1.4.13)\r\n", - "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", + "Collecting probabilistic_model\r\n", + " Using cached probabilistic_model-1.5.18-py3-none-any.whl (30 kB)\r\n", + "Requirement already satisfied, skipping upgrade: portion>=2.4.1 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.4.1)\r\n", "Requirement already satisfied, skipping upgrade: random-events>=1.2.5 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (1.2.5)\r\n", + "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", "Requirement already satisfied, skipping upgrade: anytree>=2.9.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.9.0)\r\n", - "Requirement already satisfied, skipping upgrade: portion>=2.4.1 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.4.1)\r\n", - "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n", + "Requirement already satisfied, skipping upgrade: sortedcontainers~=2.2 in /home/dprueser/.local/lib/python3.8/site-packages (from portion>=2.4.1->probabilistic_model) (2.4.0)\r\n", "Requirement already satisfied, skipping upgrade: tenacity>=6.2.0 in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (8.2.3)\r\n", + "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n", "Requirement already satisfied, skipping upgrade: six in /usr/lib/python3/dist-packages (from anytree>=2.9.0->probabilistic_model) (1.14.0)\r\n", - "Requirement already satisfied, skipping upgrade: sortedcontainers~=2.2 in /home/dprueser/.local/lib/python3.8/site-packages (from portion>=2.4.1->probabilistic_model) (2.4.0)\r\n" + "Installing collected packages: probabilistic-model\r\n", + " Attempting uninstall: probabilistic-model\r\n", + " Found existing installation: probabilistic-model 1.5.17\r\n", + " Uninstalling probabilistic-model-1.5.17:\r\n", + " Successfully uninstalled probabilistic-model-1.5.17\r\n", + "Successfully installed probabilistic-model-1.5.18\r\n" ] } ], @@ -53,8 +60,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:13:59.230429151Z", - "start_time": "2023-12-13T10:13:58.332536124Z" + "end_time": "2024-01-05T11:39:36.023993906Z", + "start_time": "2024-01-05T11:39:35.047597162Z" } }, "outputs": [ @@ -67,12 +74,13 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1702462438.857886]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702462438.862737]: Failed to import Giskard messages\n" + "[WARN] [1704454775.649694]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704454775.654652]: Failed to import Giskard messages\n" ] } ], "source": [ + "from pycram.designators.actions.actions import ParkArmsActionPerformable, MoveTorsoActionPerformable\n", "from tf import transformations\n", "import itertools\n", "from typing import Optional, List, Tuple\n", @@ -189,13 +197,13 @@ " # try to execute a grasping plan\n", " with simulated_robot:\n", "\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform()\n", " # navigate to sampled position\n", " NavigateAction([Pose(position, orientation)]).resolve().perform()\n", "\n", " # move torso\n", " height = np.random.uniform(0., 0.33, 1)[0]\n", - " MoveTorsoAction.Action(height).perform()\n", + " MoveTorsoActionPerformable(height).perform()\n", "\n", " # try to pick it up\n", " try:\n", @@ -241,8 +249,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.758891855Z", - "start_time": "2023-12-13T10:13:59.232324147Z" + "end_time": "2024-01-05T11:41:21.098968043Z", + "start_time": "2024-01-05T11:39:36.024974459Z" } }, "outputs": [ @@ -262,9 +270,9 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - " 45%|████▍ | 430/960 [00:50<00:58, 9.02it/s, success_rate=0.0791]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_shoulder_lift_link (parent map) at time 1702462489.281120 according to authority default_authority\n", + " 38%|███▊ | 369/960 [00:40<01:06, 8.92it/s, success_rate=0.0867]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/l_gripper_motor_screw_link (parent map) at time 1704454816.991220 according to authority default_authority\n", " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", - "100%|██████████| 960/960 [01:51<00:00, 9.30it/s, success_rate=0.0854]" + "100%|██████████| 960/960 [01:44<00:00, 9.47it/s, success_rate=0.0854]" ] } ], @@ -290,8 +298,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.844627649Z", - "start_time": "2023-12-13T10:15:50.794715519Z" + "end_time": "2024-01-05T11:41:21.193149641Z", + "start_time": "2024-01-05T11:41:21.142076887Z" } }, "outputs": [], @@ -317,8 +325,8 @@ "metadata": { "scrolled": true, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.932190158Z", - "start_time": "2023-12-13T10:15:50.846547804Z" + "end_time": "2024-01-05T11:41:21.278910069Z", + "start_time": "2024-01-05T11:41:21.194743363Z" } }, "outputs": [], @@ -357,8 +365,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2023-12-13T10:15:50.983050833Z", - "start_time": "2023-12-13T10:15:50.939401673Z" + "end_time": "2024-01-05T11:41:21.326300043Z", + "start_time": "2024-01-05T11:41:21.284443546Z" } }, "outputs": [ @@ -437,8 +445,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:15:51.329697221Z", - "start_time": "2023-12-13T10:15:50.982828383Z" + "end_time": "2024-01-05T11:41:21.658478914Z", + "start_time": "2024-01-05T11:41:21.326092672Z" } }, "outputs": [ @@ -446,7 +454,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "100%|██████████| 960/960 [01:51<00:00, 8.59it/s, success_rate=0.0854]\n" + "100%|██████████| 960/960 [01:45<00:00, 9.11it/s, success_rate=0.0854]\n" ] }, { @@ -475,10 +483,12 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 8, "metadata": { + "collapsed": false, "ExecuteTime": { - "start_time": "2023-12-13T10:15:51.329714184Z" + "end_time": "2024-01-05T11:46:59.969239637Z", + "start_time": "2024-01-05T11:41:21.659115424Z" } }, "outputs": [ @@ -489,11 +499,11 @@ "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", @@ -502,6 +512,7 @@ } ], "source": [ + "from pycram.designators.actions.actions import NavigateActionPerformable, PickUpActionPerformable\n", "from pycram.resolver import JPTCostmapLocation\n", "\n", "\n", @@ -516,13 +527,11 @@ "cml.visualize()\n", "with simulated_robot:\n", " for sample in iter(cml):\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", - " NavigateAction.Action(sample.pose).perform()\n", - " MoveTorsoAction.Action(sample.torso_height).perform()\n", + " ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform()\n", + " NavigateActionPerformable(sample.pose).perform()\n", + " MoveTorsoActionPerformable(sample.torso_height).perform()\n", " try:\n", - " PickUpAction.Action(\n", - " ObjectDesignatorDescription(types=[pycram.enums.ObjectType.MILK]).resolve(),\n", - " arm=sample.reachable_arm, grasp=sample.grasp).perform()\n", + " PickUpActionPerformable(ObjectDesignatorDescription(types=[pycram.enums.ObjectType.MILK]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform()\n", " # time.sleep(5)\n", " except pycram.plan_failures.PlanFailure as p:\n", " continue\n", diff --git a/examples/minimal_task_tree.ipynb b/examples/minimal_task_tree.ipynb index 1ac203580..ff4839c44 100644 --- a/examples/minimal_task_tree.ipynb +++ b/examples/minimal_task_tree.ipynb @@ -16,11 +16,25 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:45:06.870087Z", - "start_time": "2023-04-28T09:45:06.870015Z" + "end_time": "2024-01-05T11:47:15.273850937Z", + "start_time": "2024-01-05T11:47:14.439086041Z" } }, - "outputs": [], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "pybullet build time: May 20 2022 19:44:17\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1704455235.033913]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704455235.039023]: Failed to import Giskard messages\n" + ] + } + ], "source": [ "from pycram.bullet_world import BulletWorld\n", "from pycram.robot_descriptions import robot_description\n", @@ -48,8 +62,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:32:49.441717Z", - "start_time": "2023-04-27T17:32:49.441589Z" + "end_time": "2024-01-05T11:47:21.361713709Z", + "start_time": "2024-01-05T11:47:15.370855034Z" } }, "outputs": [ @@ -57,19 +71,19 @@ "name": "stderr", "output_type": "stream", "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", + "Scalar element defined multiple times: limit\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "Scalar element defined multiple times: limit\n", "Scalar element defined multiple times: limit\n" ] } @@ -98,8 +112,8 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:32:58.124468Z", - "start_time": "2023-04-27T17:32:58.124325Z" + "end_time": "2024-01-05T11:47:35.217688853Z", + "start_time": "2024-01-05T11:47:21.361234790Z" } }, "outputs": [], @@ -107,7 +121,7 @@ "@pycram.task.with_tree\n", "def plan():\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " MoveTorsoAction([0.3]).resolve().perform()\n", " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", " pickup_arm = pickup_pose.reachable_arms[0]\n", @@ -126,7 +140,7 @@ "\n", " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "plan()\n" ] @@ -143,8 +157,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-04-27T17:33:02.572319Z", - "start_time": "2023-04-27T17:33:02.572253Z" + "end_time": "2024-01-05T11:47:35.222415842Z", + "start_time": "2024-01-05T11:47:35.217889291Z" } }, "outputs": [ @@ -154,15 +168,25 @@ "text": [ "no_operation()\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -183,8 +207,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:45:59.592864Z", - "start_time": "2023-04-28T09:45:58.928492Z" + "end_time": "2024-01-05T11:47:49.422118950Z", + "start_time": "2024-01-05T11:47:35.221784522Z" } }, "outputs": [ @@ -194,25 +218,45 @@ "text": [ "no_operation()\n", "├── plan()\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(MoveTorsoAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PickUpAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PlaceAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ └── perform(ParkArmsAction, )\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(MoveTorsoActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PickUpActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PlaceActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ └── perform(ParkArmsActionPerformable)\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -234,8 +278,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:37.041421Z", - "start_time": "2023-04-28T09:46:36.999813Z" + "end_time": "2024-01-05T11:47:49.434500008Z", + "start_time": "2024-01-05T11:47:49.422026208Z" } }, "outputs": [ @@ -246,25 +290,45 @@ "simulation()\n", "no_operation()\n", "├── plan()\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(MoveTorsoAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PickUpAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ ├── perform(NavigateAction, )\n", - "│ ├── perform(PlaceAction, )\n", - "│ ├── perform(ParkArmsAction, )\n", - "│ └── perform(ParkArmsAction, )\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(MoveTorsoActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PickUpActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ ├── perform(NavigateActionPerformable)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(PlaceActionPerformable)\n", + "│ │ ├── perform(Motion)\n", + "│ │ ├── perform(Motion)\n", + "│ │ └── perform(Motion)\n", + "│ ├── perform(ParkArmsActionPerformable)\n", + "│ └── perform(ParkArmsActionPerformable)\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PickUpAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " ├── perform(PlaceAction, )\n", - " ├── perform(ParkArmsAction, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -286,8 +350,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:45.637721Z", - "start_time": "2023-04-28T09:46:45.635535Z" + "end_time": "2024-01-05T11:47:49.438410783Z", + "start_time": "2024-01-05T11:47:49.433036544Z" } }, "outputs": [ @@ -297,15 +361,25 @@ "text": [ "no_operation()\n", "+-- plan()\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(MoveTorsoAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PickUpAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PlaceAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " +-- perform(ParkArmsAction, )\n" + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(MoveTorsoActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PickUpActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PlaceActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " +-- perform(ParkArmsActionPerformable)\n" ] } ], @@ -326,8 +400,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:48.259716Z", - "start_time": "2023-04-28T09:46:48.233474Z" + "end_time": "2024-01-05T11:47:57.076211143Z", + "start_time": "2024-01-05T11:47:49.437672197Z" } }, "outputs": [ @@ -337,15 +411,25 @@ "text": [ "no_operation()\n", "+-- plan()\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(MoveTorsoAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PickUpAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " |-- perform(NavigateAction, )\n", - " |-- perform(PlaceAction, )\n", - " |-- perform(ParkArmsAction, )\n", - " +-- perform(ParkArmsAction, )\n" + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(MoveTorsoActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PickUpActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " |-- perform(NavigateActionPerformable)\n", + " | +-- perform(Motion)\n", + " |-- perform(PlaceActionPerformable)\n", + " | |-- perform(Motion)\n", + " | |-- perform(Motion)\n", + " | +-- perform(Motion)\n", + " |-- perform(ParkArmsActionPerformable)\n", + " +-- perform(ParkArmsActionPerformable)\n" ] } ], @@ -368,8 +452,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:52.656020Z", - "start_time": "2023-04-28T09:46:52.653083Z" + "end_time": "2024-01-05T11:47:57.081178616Z", + "start_time": "2024-01-05T11:47:57.077818445Z" } }, "outputs": [ @@ -378,9 +462,9 @@ "output_type": "stream", "text": [ "Code: plan() \n", - " start_time: 2023-11-08 14:01:12.716969 \n", + " start_time: 2024-01-05 12:47:21.363057 \n", " Status: TaskStatus.SUCCEEDED \n", - " end_time: 2023-11-08 14:01:24.675108 \n", + " end_time: 2024-01-05 12:47:35.214734 \n", " \n" ] } @@ -401,8 +485,8 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:55.385481Z", - "start_time": "2023-04-28T09:46:55.381316Z" + "end_time": "2024-01-05T11:47:57.090325704Z", + "start_time": "2024-01-05T11:47:57.081062518Z" } }, "outputs": [ @@ -431,8 +515,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:46:57.445908Z", - "start_time": "2023-04-28T09:46:57.443154Z" + "end_time": "2024-01-05T11:47:57.130889801Z", + "start_time": "2024-01-05T11:47:57.129989892Z" } }, "outputs": [ @@ -467,8 +551,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:47:00.292660Z", - "start_time": "2023-04-28T09:47:00.287579Z" + "end_time": "2024-01-05T11:47:57.131271385Z", + "start_time": "2024-01-05T11:47:57.130193323Z" } }, "outputs": [ @@ -479,9 +563,9 @@ "no_operation()\n", "+-- failing_plan()\n", "Code: failing_plan() \n", - " start_time: 2023-11-08 14:02:00.257042 \n", + " start_time: 2024-01-05 12:47:57.088480 \n", " Status: TaskStatus.FAILED \n", - " end_time: 2023-11-08 14:02:00.257507 \n", + " end_time: 2024-01-05 12:47:57.089024 \n", " \n" ] } @@ -496,8 +580,8 @@ "execution_count": 13, "metadata": { "ExecuteTime": { - "end_time": "2023-04-28T09:47:03.581366Z", - "start_time": "2023-04-28T09:47:03.447150Z" + "end_time": "2024-01-05T11:47:57.328473210Z", + "start_time": "2024-01-05T11:47:57.130261104Z" } }, "outputs": [], diff --git a/examples/orm_example.ipynb b/examples/orm_example.ipynb index 20a3a4532..dc0b3e711 100644 --- a/examples/orm_example.ipynb +++ b/examples/orm_example.ipynb @@ -15,14 +15,14 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:05.213398212Z", - "start_time": "2023-12-13T10:22:05.061219318Z" + "end_time": "2024-01-05T11:48:06.779230216Z", + "start_time": "2024-01-05T11:48:06.588261981Z" } }, "outputs": [ { "data": { - "text/plain": "" + "text/plain": "" }, "execution_count": 1, "metadata": {}, @@ -50,8 +50,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:05.758910570Z", - "start_time": "2023-12-13T10:22:05.211301707Z" + "end_time": "2024-01-05T11:48:07.264463765Z", + "start_time": "2024-01-05T11:48:06.770051795Z" } }, "outputs": [ @@ -64,8 +64,8 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1702462925.736253]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702462925.741150]: Failed to import Giskard messages\n" + "[WARN] [1704455287.243634]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704455287.248237]: Failed to import Giskard messages\n" ] } ], @@ -88,8 +88,8 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.532059166Z", - "start_time": "2023-12-13T10:22:05.759939989Z" + "end_time": "2024-01-05T11:48:27.697221588Z", + "start_time": "2024-01-05T11:48:07.267874656Z" } }, "outputs": [ @@ -119,24 +119,24 @@ "text": [ "no_operation()\n", "└── plan()\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(MoveTorsoAction, )\n", - " ├── perform(NavigateAction, )\n", - " │ └── perform(MoveMotion, )\n", - " ├── perform(PickUpAction, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ └── perform(MoveTCPMotion, )\n", - " ├── perform(ParkArmsAction, )\n", - " ├── perform(NavigateAction, )\n", - " │ └── perform(MoveMotion, )\n", - " ├── perform(PlaceAction, )\n", - " │ ├── perform(MoveTCPMotion, )\n", - " │ ├── perform(MoveGripperMotion, )\n", - " │ └── perform(MoveTCPMotion, )\n", - " └── perform(ParkArmsAction, )\n" + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(MoveTorsoActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PickUpActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " ├── perform(ParkArmsActionPerformable)\n", + " ├── perform(NavigateActionPerformable)\n", + " │ └── perform(Motion)\n", + " ├── perform(PlaceActionPerformable)\n", + " │ ├── perform(Motion)\n", + " │ ├── perform(Motion)\n", + " │ └── perform(Motion)\n", + " └── perform(ParkArmsActionPerformable)\n" ] } ], @@ -165,7 +165,7 @@ "@with_tree\n", "def plan():\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " MoveTorsoAction([0.3]).resolve().perform()\n", " pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve()\n", " pickup_arm = pickup_pose.reachable_arms[0]\n", @@ -182,7 +182,7 @@ "\n", " PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "plan()\n", "\n", @@ -204,8 +204,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.715646672Z", - "start_time": "2023-12-13T10:22:25.534065783Z" + "end_time": "2024-01-05T11:48:27.882423457Z", + "start_time": "2024-01-05T11:48:27.695892994Z" } }, "outputs": [ @@ -213,12 +213,12 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 114.02it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 112.65it/s]\n" ] }, { "data": { - "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" + "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" }, "execution_count": 4, "metadata": {}, @@ -241,8 +241,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.720098203Z", - "start_time": "2023-12-13T10:22:25.717086864Z" + "end_time": "2024-01-05T11:48:27.887896771Z", + "start_time": "2024-01-05T11:48:27.883057209Z" } }, "outputs": [ @@ -250,7 +250,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "ProcessMetaData(id=1, created_at=datetime.datetime(2023, 12, 13, 10, 22, 25), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='3422aa2a9097bcb087795fccf89e5bd128ab21dd')\n" + "ProcessMetaData(id=1, created_at=datetime.datetime(2024, 1, 5, 11, 48, 27), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='be748996363cb497af6784f6f2aaeffcb2fb1c42')\n" ] } ], @@ -271,8 +271,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.774954933Z", - "start_time": "2023-12-13T10:22:25.719710555Z" + "end_time": "2024-01-05T11:48:27.933255911Z", + "start_time": "2024-01-05T11:48:27.886082312Z" } }, "outputs": [ @@ -302,8 +302,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.775554799Z", - "start_time": "2023-12-13T10:22:25.774858881Z" + "end_time": "2024-01-05T11:48:27.933794333Z", + "start_time": "2024-01-05T11:48:27.930158041Z" } }, "outputs": [ @@ -339,8 +339,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.775915781Z", - "start_time": "2023-12-13T10:22:25.774973642Z" + "end_time": "2024-01-05T11:48:27.934174898Z", + "start_time": "2024-01-05T11:48:27.930328964Z" } }, "outputs": [ @@ -348,7 +348,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "Pose(id=7, time=datetime.datetime(2023, 12, 13, 10, 22, 12, 75413), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" + "Pose(id=7, time=datetime.datetime(2024, 1, 5, 11, 48, 13, 104805), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" ] } ], @@ -384,8 +384,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.776819211Z", - "start_time": "2023-12-13T10:22:25.775011202Z" + "end_time": "2024-01-05T11:48:27.992321580Z", + "start_time": "2024-01-05T11:48:27.930434353Z" } }, "outputs": [ @@ -393,25 +393,25 @@ "name": "stdout", "output_type": "stream", "text": [ - "TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144323), end_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 650626), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=4, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 650720), end_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 154581), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704577), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214635), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=6, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704616), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214628), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 13, 704577), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214635), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=8, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 385460), end_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 894615), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 894706), end_time=datetime.datetime(2023, 12, 13, 11, 22, 15, 402177), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 15, 552840), end_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 62542), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 62656), end_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 570383), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=12, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 16, 703869), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 214127), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 14, 214734), end_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504156), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=13, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 17, 504274), end_time=datetime.datetime(2023, 12, 13, 11, 22, 18, 13609), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957228), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462603), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957259), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462584), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 22, 957228), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462603), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 463717), end_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 974361), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=18, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 974450), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 482580), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=19, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 482694), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992063), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 23, 462711), end_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992076), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=20, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 24, 992156), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498594), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 12, 144277), end_time=datetime.datetime(2023, 12, 13, 11, 22, 25, 498607), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2023, 12, 13, 11, 22, 5, 734143), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" + "TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167962), end_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 674674), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=4, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 674769), end_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 177846), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742815), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288620), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=6, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742855), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288611), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742815), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288620), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=8, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 417504), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 929845), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 929939), end_time=datetime.datetime(2024, 1, 5, 12, 48, 16, 435586), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 16, 568469), end_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 76882), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 77011), end_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 589849), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=12, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 720082), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 228417), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=13, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 520072), end_time=datetime.datetime(2024, 1, 5, 12, 48, 19, 27331), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115462), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625686), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115512), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625678), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115462), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625686), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 626977), end_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 135284), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=18, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 135369), end_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 642910), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=19, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 643067), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153860), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=20, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153953), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660007), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" ] } ], @@ -434,14 +434,15 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.827781799Z", - "start_time": "2023-12-13T10:22:25.782671460Z" + "end_time": "2024-01-05T11:48:27.993029221Z", + "start_time": "2024-01-05T11:48:27.956265531Z" } }, "outputs": [], "source": [ "from sqlalchemy.orm import Mapped, mapped_column\n", "from pycram.orm.action_designator import Action\n", + "from dataclasses import dataclass\n", "\n", "\n", "# define ORM class from pattern in every pycram.orm class\n", @@ -452,32 +453,24 @@ " text: Mapped[str] \n", "\n", "# define brand new action designator\n", - "class Saying(ActionDesignatorDescription):\n", + "\n", + "@dataclass \n", + "class SayingActionPerformable(ActionDesignatorDescription.Action):\n", " \n", - " @dataclasses.dataclass \n", - " class Action(ActionDesignatorDescription.Action):\n", + " text: str\n", " \n", - " text: str\n", - " \n", - " @with_tree\n", - " def perform(self) -> Any:\n", - " print(self.text)\n", - " \n", - " def to_sql(self) -> ORMSaying:\n", - " return ORMSaying(self.text)\n", + " @with_tree\n", + " def perform(self) -> None:\n", + " print(self.text)\n", "\n", - " def insert(self, session: Session, *args, **kwargs) -> ORMSaying:\n", - " action = super().insert(session)\n", - " session.add(action)\n", - " session.commit()\n", - " return action\n", - " \n", - " def __init__(self, texts: List[str], resolver=None):\n", - " super().__init__(resolver)\n", - " self.texts = texts\n", + " def to_sql(self) -> ORMSaying:\n", + " return ORMSaying(self.text)\n", "\n", - " def ground(self) -> Action:\n", - " return self.Action(self.texts[0])" + " def insert(self, session: Session, *args, **kwargs) -> ORMSaying:\n", + " action = super().insert(session)\n", + " session.add(action)\n", + " session.commit()\n", + " return action\n" ] }, { @@ -492,8 +485,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:25.828275817Z", - "start_time": "2023-12-13T10:22:25.826744665Z" + "end_time": "2024-01-05T11:48:27.993997340Z", + "start_time": "2024-01-05T11:48:27.956444747Z" } }, "outputs": [], @@ -513,8 +506,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:26.207983261Z", - "start_time": "2023-12-13T10:22:25.826841295Z" + "end_time": "2024-01-05T11:48:28.491240632Z", + "start_time": "2024-01-05T11:48:27.965003339Z" } }, "outputs": [ @@ -529,13 +522,13 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 122.69it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 77.00it/s]\n" ] } ], "source": [ "# create a saying action and insert it\n", - "Saying.Action(\"Patchie, Patchie; Where is my Patchie?\").perform()\n", + "SayingActionPerformable(\"Patchie, Patchie; Where is my Patchie?\").perform()\n", "pycram.task.task_tree.root.insert(session)\n", "session.commit()\n", "\n", @@ -555,8 +548,8 @@ "execution_count": 13, "metadata": { "ExecuteTime": { - "end_time": "2023-12-13T10:22:26.214535062Z", - "start_time": "2023-12-13T10:22:26.209490452Z" + "end_time": "2024-01-05T11:48:28.526329436Z", + "start_time": "2024-01-05T11:48:28.478332427Z" } }, "outputs": [ diff --git a/examples/orm_querying_examples.ipynb b/examples/orm_querying_examples.ipynb index 28f52f63d..64f78edb4 100644 --- a/examples/orm_querying_examples.ipynb +++ b/examples/orm_querying_examples.ipynb @@ -36,12 +36,13 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1702467193.499071]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1702467193.504366]: Failed to import Giskard messages\n" + "[WARN] [1704455774.294953]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704455774.300327]: Failed to import Giskard messages\n" ] } ], "source": [ + "from pycram.designators.actions.actions import MoveTorsoActionPerformable, ParkArmsActionPerformable\n", "from tf import transformations\n", "import itertools\n", "import time\n", @@ -167,13 +168,13 @@ " # try to execute a grasping plan\n", " with simulated_robot:\n", "\n", - " ParkArmsAction.Action(pycram.enums.Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform()\n", " # navigate to sampled position\n", " NavigateAction([Pose(position, orientation)]).resolve().perform()\n", "\n", " # move torso\n", " height = np.random.uniform(0., 0.33, 1)[0]\n", - " MoveTorsoAction.Action(height).perform()\n", + " MoveTorsoActionPerformable(height).perform()\n", "\n", " # try to pick it up\n", " try:\n", @@ -205,8 +206,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:33:13.874158209Z", - "start_time": "2023-12-13T11:33:12.871008643Z" + "end_time": "2024-01-05T11:56:14.747051902Z", + "start_time": "2024-01-05T11:56:13.637965477Z" } }, "id": "dc0c9e6f15f126a3" @@ -242,7 +243,9 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "100%|██████████| 960/960 [01:41<00:00, 9.87it/s, success_rate=0.0854]" + " 74%|███████▍ | 708/960 [01:31<00:29, 8.48it/s, success_rate=0.0819]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_gripper_palm_link (parent map) at time 1704455865.925491 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "100%|██████████| 960/960 [02:03<00:00, 7.85it/s, success_rate=0.0854]" ] } ], @@ -258,8 +261,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.371037890Z", - "start_time": "2023-12-13T11:33:13.874635425Z" + "end_time": "2024-01-05T11:58:18.210175597Z", + "start_time": "2024-01-05T11:56:14.749214037Z" } }, "id": "f2dcacd942218899" @@ -343,8 +346,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.371818556Z", - "start_time": "2023-12-13T11:34:55.371020667Z" + "end_time": "2024-01-05T11:58:18.210316772Z", + "start_time": "2024-01-05T11:58:18.210052650Z" } }, "id": "b17c6366f317dd7" @@ -442,8 +445,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2023-12-13T11:34:55.423112328Z", - "start_time": "2023-12-13T11:34:55.371312120Z" + "end_time": "2024-01-05T11:58:18.254184939Z", + "start_time": "2024-01-05T11:58:18.210187006Z" } }, "id": "a89d1a0f2a933475" diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 20f1beb48..7a2c312ea 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -1,9 +1,8 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -import dataclasses +from dataclasses import dataclass, field, fields from abc import ABC, abstractmethod -from copy import copy from inspect import isgenerator, isgeneratorfunction from sqlalchemy.orm.session import Session @@ -367,7 +366,7 @@ class MotionDesignatorDescription(DesignatorDescription): Parent class of motion designator descriptions. """ - @dataclasses.dataclass + @dataclass class Motion: """ Resolved motion designator which can be performed @@ -465,21 +464,21 @@ class ActionDesignatorDescription(DesignatorDescription): Descriptions hold possible parameter ranges for action designators. """ - @dataclasses.dataclass + @dataclass class Action: """ The performable designator with a single element for each list of possible parameter. """ - robot_position: Pose = dataclasses.field(init=False) + robot_position: Pose = field(init=False) """ The position of the robot at the start of the action. """ - robot_torso_height: float = dataclasses.field(init=False) + robot_torso_height: float = field(init=False) """ The torso height of the robot at the start of the action. """ - robot_type: str = dataclasses.field(init=False) + robot_type: str = field(init=False) """ The type of the robot at the start of the action. """ @@ -546,7 +545,7 @@ class LocationDesignatorDescription(DesignatorDescription): Parent class of location designator descriptions. """ - @dataclasses.dataclass + @dataclass class Location: """ Resolved location that represents a specific point in the world which satisfies the constraints of the location @@ -585,7 +584,7 @@ class ObjectDesignatorDescription(DesignatorDescription): Descriptions hold possible parameter ranges for object designators. """ - @dataclasses.dataclass + @dataclass class Object: """ A single element that fits the description. @@ -606,7 +605,7 @@ class Object: Reference to the BulletWorld object """ - _pose: Optional[Callable] = dataclasses.field(init=False) + _pose: Optional[Callable] = field(init=False) """ A callable returning the pose of this object. The _pose member is used overwritten for data copies which will not update when the original bullet_world_object is moved. @@ -645,7 +644,7 @@ def insert(self, session: Session) -> ORMObjectDesignator: session.commit() return obj - def data_copy(self) -> 'ObjectDesignatorDescription.Object': + def frozen_copy(self) -> 'ObjectDesignatorDescription.Object': """ :return: A copy containing only the fields of this class. The BulletWorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when @@ -678,7 +677,7 @@ def pose(self, value: Callable): def __repr__(self): return self.__class__.__qualname__ + f"(" + ', '.join( - [f"{f.name}={self.__getattribute__(f.name)}" for f in dataclasses.fields(self)] + [ + [f"{f.name}={self.__getattribute__(f.name)}" for f in fields(self)] + [ f"pose={self.pose}"]) + ')' def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index ddc9b24ab..f8b9d347f 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1,31 +1,17 @@ -import dataclasses import itertools -import time -from typing import List, Optional, Any, Tuple, Union - -import sqlalchemy.orm - -from .location_designator import CostmapLocation +from typing import Union from .motion_designator import * -from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from ..local_transformer import LocalTransformer -from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, - PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, - MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, - Action as ORMAction, LookAtAction as ORMLookAtAction, - DetectAction as ORMDetectAction, TransportAction as ORMTransportAction, - OpenAction as ORMOpenAction, CloseAction as ORMCloseAction, - GraspingAction as ORMGraspingAction) - -from ..orm.base import Quaternion, Position, Base, RobotState, ProcessMetaData -from ..plan_failures import ObjectUnfetchable, ReachabilityFailure -from ..robot_descriptions import robot_description -from ..task import with_tree +from .object_designator import ObjectDesignatorDescription, ObjectPart from ..enums import Arms from ..designator import ActionDesignatorDescription -from ..bullet_world import BulletWorld from ..pose import Pose -from ..helper import multiply_quaternions +from pycram.designators.actions.actions import (ParkArmsActionPerformable, MoveTorsoActionPerformable, + SetGripperActionPerformable, GripActionPerformable, + PlaceActionPerformable, PickUpActionPerformable, + NavigateActionPerformable, TransportActionPerformable, + LookAtActionPerformable, DetectActionPerformable, OpenActionPerformable, + CloseActionPerformable, GraspingActionPerformable, + ReleaseActionPerformable) class MoveTorsoAction(ActionDesignatorDescription): @@ -33,30 +19,6 @@ class MoveTorsoAction(ActionDesignatorDescription): Action Designator for Moving the torso of the robot up and down """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - """ - Performable Move Torso Action designator. - """ - - position: float - """ - Target position of the torso joint - """ - - @with_tree - def perform(self) -> None: - MoveJointsMotion([robot_description.torso_joint], [self.position]).resolve().perform() - - def to_sql(self) -> ORMMoveTorsoAction: - return ORMMoveTorsoAction(self.position) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMMoveTorsoAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, positions: List[float], resolver=None): """ Create a designator description to move the torso of the robot up and down. @@ -67,13 +29,13 @@ def __init__(self, positions: List[float], resolver=None): super().__init__(resolver) self.positions: List[float] = positions - def ground(self) -> Action: + def ground(self) -> MoveTorsoActionPerformable: """ Creates a performable action designator with the first element from the list of possible torso heights. :return: A performable action designator """ - return self.Action(self.positions[0]) + return MoveTorsoActionPerformable(self.positions[0]) def __iter__(self): """ @@ -82,7 +44,7 @@ def __iter__(self): :return: A performable action designator """ for position in self.positions: - yield self.Action(position) + yield MoveTorsoActionPerformable(position) class SetGripperAction(ActionDesignatorDescription): @@ -90,30 +52,6 @@ class SetGripperAction(ActionDesignatorDescription): Set the gripper state of the robot """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - """ - The gripper that should be set - """ - motion: str - """ - The motion that should be set on the gripper - """ - - @with_tree - def perform(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).resolve().perform() - - def to_sql(self) -> ORMSetGripperAction: - return ORMSetGripperAction(self.gripper, self.motion) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMSetGripperAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, grippers: List[str], motions: List[str], resolver=None): """ Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. @@ -126,13 +64,13 @@ def __init__(self, grippers: List[str], motions: List[str], resolver=None): self.grippers: List[str] = grippers self.motions: List[str] = motions - def ground(self) -> Action: + def ground(self) -> SetGripperActionPerformable: """ Default resolver that returns a performable designator with the first element in the grippers and motions list. :return: A performable designator """ - return self.Action(self.grippers[0], self.motions[0]) + return SetGripperActionPerformable(self.grippers[0], self.motions[0]) def __iter__(self): """ @@ -141,7 +79,7 @@ def __iter__(self): :return: A performable designator with a combination of gripper and motion """ for parameter_combination in itertools.product(self.grippers, self.motions): - yield self.Action(*parameter_combination) + yield SetGripperActionPerformable(*parameter_combination) class ReleaseAction(ActionDesignatorDescription): @@ -151,29 +89,14 @@ class ReleaseAction(ActionDesignatorDescription): Note: This action can not be used yet. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - object_designator: ObjectDesignatorDescription.Object - - @with_tree - def perform(self) -> Any: - raise NotImplementedError() - - def to_sql(self) -> Base: - raise NotImplementedError() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> Base: - raise NotImplementedError() - def __init__(self, grippers: List[str], object_designator_description: ObjectDesignatorDescription, resolver=None): super().__init__(resolver) self.grippers: List[str] = grippers self.object_designator_description = object_designator_description - def ground(self) -> Action: - return self.Action(self.grippers[0], self.object_designator_description.ground()) + def ground(self) -> ReleaseActionPerformable: + return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) class GripAction(ActionDesignatorDescription): @@ -187,22 +110,6 @@ class GripAction(ActionDesignatorDescription): Note: This action can not be used yet. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - gripper: str - object_designator: ObjectDesignatorDescription.Object - effort: float - - @with_tree - def perform(self) -> Any: - raise NotImplementedError() - - def to_sql(self) -> Base: - raise NotImplementedError() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> Base: - raise NotImplementedError() - def __init__(self, grippers: List[str], object_designator_description: ObjectDesignatorDescription, efforts: List[float], resolver=None): super().__init__(resolver) @@ -210,8 +117,8 @@ def __init__(self, grippers: List[str], object_designator_description: ObjectDes self.object_designator_description: ObjectDesignatorDescription = object_designator_description self.efforts: List[float] = efforts - def ground(self) -> Action: - return self.Action(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) + def ground(self) -> GripActionPerformable: + return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) class ParkArmsAction(ActionDesignatorDescription): @@ -219,37 +126,6 @@ class ParkArmsAction(ActionDesignatorDescription): Park the arms of the robot. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - arm: Arms - """ - Entry from the enum for which arm should be parked - """ - - @with_tree - def perform(self) -> None: - # create the keyword arguments - kwargs = dict() - - # add park left arm if wanted - if self.arm in [Arms.LEFT, Arms.BOTH]: - kwargs["left_arm_config"] = "park" - - # add park right arm if wanted - if self.arm in [Arms.RIGHT, Arms.BOTH]: - kwargs["right_arm_config"] = "park" - MoveArmJointsMotion(**kwargs).resolve().perform() - - def to_sql(self) -> ORMParkArmsAction: - return ORMParkArmsAction(self.arm.name) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMParkArmsAction: - action = super().insert(session) - session.add(action) - session.commit() - return action - def __init__(self, arms: List[Arms], resolver=None): """ Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms @@ -260,13 +136,13 @@ def __init__(self, arms: List[Arms], resolver=None): super().__init__(resolver) self.arms: List[Arms] = arms - def ground(self) -> Action: + def ground(self) -> ParkArmsActionPerformable: """ Default resolver that returns a performable designator with the first element of the list of possible arms :return: A performable designator """ - return self.Action(self.arms[0]) + return ParkArmsActionPerformable(self.arms[0]) class PickUpAction(ActionDesignatorDescription): @@ -274,110 +150,6 @@ class PickUpAction(ActionDesignatorDescription): Designator to let the robot pick up an object. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be picked up - """ - - arm: str - """ - The arm that should be used for pick up - """ - - grasp: str - """ - The grasp that should be used. For example, 'left' or 'right' - """ - - object_at_execution: Optional[ObjectDesignatorDescription.Object] = dataclasses.field(init=False) - """ - The object at the time this Action got created. It is used to be a static, information holding entity. It is - not updated when the BulletWorld object is changed. - """ - - @with_tree - def perform(self) -> None: - # Store the object's data copy at execution - self.object_at_execution = self.object_designator.data_copy() - robot = BulletWorld.robot - # Retrieve object and robot from designators - object = self.object_designator.bullet_world_object - # Get grasp orientation and target pose - grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) - # oTm = Object Pose in Frame map - oTm = object.get_pose() - # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_to_object_frame(oTm, object) - # Adjust the pose according to the special knowledge of the object designator - adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) - # Transform the adjusted pose to the map frame - adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") - # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper - ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, - adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], - grasp) - - # Set the orientation of the object pose by grasp in MAP - adjusted_oTm.orientation.x = ori[0] - adjusted_oTm.orientation.y = ori[1] - adjusted_oTm.orientation.z = ori[2] - adjusted_oTm.orientation.w = ori[3] - - # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh - # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" - gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) - # First rotate the gripper, so the further calculations makes sense - tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - tmp_for_rotate_pose.pose.position.x = 0 - tmp_for_rotate_pose.pose.position.y = 0 - tmp_for_rotate_pose.pose.position.z = -0.1 - gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") - - #Perform Gripper Rotate - # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) - # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() - - oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented - prepose = object.local_transformer.transform_pose(oTg, "map") - - # Perform the motion with the prepose and open gripper - BulletWorld.current_bullet_world.add_vis_axis(prepose) - MoveTCPMotion(prepose, self.arm).resolve().perform() - MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() - - # Perform the motion with the adjusted pose -> actual grasp and close gripper - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() - adjusted_oTm.pose.position.z += 0.03 - MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() - tool_frame = robot_description.get_tool_frame(self.arm) - robot.attach(object, tool_frame) - - # Lift object - BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() - - # Remove the vis axis from the world - BulletWorld.current_bullet_world.remove_vis_axis() - - def to_sql(self) -> ORMPickUpAction: - return ORMPickUpAction(self.arm, self.grasp) - - def insert(self, session: sqlalchemy.orm.session.Session, **kwargs) -> ORMPickUpAction: - action = super().insert(session) - - od = self.object_at_execution.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[str], grasps: List[str], resolver=None): """ @@ -395,7 +167,7 @@ def __init__(self, object_designator_description: Union[ObjectDesignatorDescrip self.arms: List[str] = arms self.grasps: List[str] = grasps - def ground(self) -> Action: + def ground(self) -> PickUpActionPerformable: """ Default resolver, returns a performable designator with the first entries from the lists of possible parameter. @@ -404,7 +176,7 @@ def ground(self) -> Action: obj_desig = self.object_designator_description if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, self.arms[0], self.grasps[0]) + return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) class PlaceAction(ActionDesignatorDescription): @@ -412,58 +184,6 @@ class PlaceAction(ActionDesignatorDescription): Places an Object at a position using an arm. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be place - """ - arm: str - """ - Arm that is currently holding the object - """ - target_location: Pose - """ - Pose in the world at which the object should be placed - """ - - @with_tree - def perform(self) -> None: - object_pose = self.object_designator.bullet_world_object.get_pose() - local_tf = LocalTransformer() - - # Transformations such that the target position is the position of the object and not the tcp - tcp_to_object = local_tf.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame( - robot_description.get_tool_frame(self.arm))) - target_diff = self.target_location.to_transform("target").inverse_times( - tcp_to_object.to_transform("object")).to_pose() - - MoveTCPMotion(target_diff, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() - BulletWorld.robot.detach(self.object_designator.bullet_world_object) - retract_pose = target_diff - retract_pose.position.x -= 0.07 - MoveTCPMotion(retract_pose, self.arm).resolve().perform() - - def to_sql(self) -> ORMPlaceAction: - return ORMPlaceAction(self.arm) - - def insert(self, session, *args, **kwargs) -> ORMPlaceAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], target_locations: List[Pose], @@ -482,7 +202,7 @@ def __init__(self, self.target_locations: List[Pose] = target_locations self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> PlaceActionPerformable: """ Default resolver that returns a performable designator with the first entries from the list of possible entries. @@ -491,7 +211,7 @@ def ground(self) -> Action: obj_desig = self.object_designator_description if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, self.arms[0], self.target_locations[0]) + return PlaceActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) class NavigateAction(ActionDesignatorDescription): @@ -499,31 +219,6 @@ class NavigateAction(ActionDesignatorDescription): Navigates the Robot to a position. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - target_location: Pose - """ - Location to which the robot should be navigated - """ - - @with_tree - def perform(self) -> None: - MoveMotion(self.target_location).resolve().perform() - - def to_sql(self) -> ORMNavigateAction: - return ORMNavigateAction() - - def insert(self, session, *args, **kwargs) -> ORMNavigateAction: - action = super().insert(session) - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, target_locations: List[Pose], resolver=None): """ Navigates the robot to a location. @@ -534,13 +229,13 @@ def __init__(self, target_locations: List[Pose], resolver=None): super().__init__(resolver) self.target_locations: List[Pose] = target_locations - def ground(self) -> Action: + def ground(self) -> NavigateActionPerformable: """ Default resolver that returns a performable designator with the first entry of possible target locations. :return: A performable designator """ - return self.Action(self.target_locations[0]) + return NavigateActionPerformable(self.target_locations[0]) class TransportAction(ActionDesignatorDescription): @@ -548,67 +243,6 @@ class TransportAction(ActionDesignatorDescription): Transports an object to a position using an arm """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be transported. - """ - arm: str - """ - Arm that should be used - """ - target_location: Pose - """ - Target Location to which the object should be transported - """ - - @with_tree - def perform(self) -> None: - robot_desig = BelieveObject(names=[robot_description.name]) - ParkArmsAction.Action(Arms.BOTH).perform() - pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), - reachable_arm=self.arm) - # Tries to find a pick-up posotion for the robot that uses the given arm - pickup_pose = None - for pose in pickup_loc: - if self.arm in pose.reachable_arms: - pickup_pose = pose - break - if not pickup_pose: - raise ObjectUnfetchable( - f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") - - NavigateAction([pickup_pose.pose]).resolve().perform() - PickUpAction.Action(self.object_designator, self.arm, "front").perform() - ParkArmsAction.Action(Arms.BOTH).perform() - try: - place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), - reachable_arm=self.arm).resolve() - except StopIteration: - raise ReachabilityFailure( - f"No location found from where the robot can reach the target location: {self.target_location}") - NavigateAction([place_loc.pose]).resolve().perform() - PlaceAction.Action(self.object_designator, self.arm, self.target_location).perform() - ParkArmsAction.Action(Arms.BOTH).perform() - - def to_sql(self) -> ORMTransportAction: - return ORMTransportAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMTransportAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - pose = self.target_location.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[str], @@ -627,17 +261,17 @@ def __init__(self, self.arms: List[str] = arms self.target_locations: List[Pose] = target_locations - def ground(self) -> Action: + def ground(self) -> TransportActionPerformable: """ Default resolver that returns a performable designator with the first entries from the lists of possible parameter. :return: A performable designator """ - obj_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() - return self.Action(obj_desig, - self.arms[0], - self.target_locations[0]) + obj_desig = self.object_designator_description \ + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object)\ + else self.object_designator_description.resolve() + + return TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) class LookAtAction(ActionDesignatorDescription): @@ -645,30 +279,6 @@ class LookAtAction(ActionDesignatorDescription): Lets the robot look at a position. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - target: Pose - """ - Position at which the robot should look, given as 6D pose - """ - - @with_tree - def perform(self) -> None: - LookingMotion(target=self.target).resolve().perform() - - def to_sql(self) -> ORMLookAtAction: - return ORMLookAtAction() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMLookAtAction: - action = super().insert(session) - - pose = self.target.insert(session) - action.pose_id = pose.id - - session.add(action) - session.commit() - return action - def __init__(self, targets: List[Pose], resolver=None): """ Moves the head of the robot such that it points towards the given target location. @@ -679,13 +289,13 @@ def __init__(self, targets: List[Pose], resolver=None): super().__init__(resolver) self.targets: List[Pose] = targets - def ground(self) -> Action: + def ground(self) -> LookAtActionPerformable: """ Default resolver that returns a performable designator with the first entry in the list of possible targets :return: A performable designator """ - return self.Action(self.targets[0]) + return LookAtActionPerformable(self.targets[0]) class DetectAction(ActionDesignatorDescription): @@ -693,31 +303,6 @@ class DetectAction(ActionDesignatorDescription): Detects an object that fits the object description and returns an object designator describing the object. """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectDesignatorDescription.Object - """ - Object designator loosely describing the object, e.g. only type. - """ - - @with_tree - def perform(self) -> Any: - return DetectingMotion(object_type=self.object_designator.type).resolve().perform() - - def to_sql(self) -> ORMDetectAction: - return ORMDetectAction() - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMDetectAction: - action = super().insert(session) - - od = self.object_designator.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None): """ Tries to detect an object in the field of view (FOV) of the robot. @@ -728,13 +313,13 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, r super().__init__(resolver) self.object_designator_description: ObjectDesignatorDescription = object_designator_description - def ground(self) -> Action: + def ground(self) -> DetectActionPerformable: """ Default resolver that returns a performable designator with the resolved object description. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve()) + return DetectActionPerformable(self.object_designator_description.resolve()) class OpenAction(ActionDesignatorDescription): @@ -744,38 +329,6 @@ class OpenAction(ActionDesignatorDescription): Can currently not be used """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be opened - """ - arm: str - """ - Arm that should be used for opening the container - """ - - @with_tree - def perform(self) -> Any: - GraspingAction.Action(self.arm, self.object_designator).perform() - OpeningMotion(self.object_designator, self.arm).resolve().perform() - - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMOpenAction: - return ORMOpenAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMOpenAction: - action = super().insert(session) - - op = self.object_designator.insert(session) - action.object_id = op.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectPart, arms: List[str], resolver=None): """ Moves the arm of the robot to open a container. @@ -788,14 +341,14 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[str], r self.object_designator_description: ObjectPart = object_designator_description self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> OpenActionPerformable: """ Default resolver that returns a performable designator with the resolved object description and the first entries from the lists of possible parameter. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve(), self.arms[0]) + return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) class CloseAction(ActionDesignatorDescription): @@ -805,38 +358,6 @@ class CloseAction(ActionDesignatorDescription): Can currently not be used """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be closed - """ - arm: str - """ - Arm that should be used for closing - """ - - @with_tree - def perform(self) -> Any: - GraspingAction.Action(self.arm, self.object_designator).perform() - ClosingMotion(self.object_designator, self.arm).resolve().perform() - - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMCloseAction: - return ORMCloseAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMCloseAction: - action = super().insert(session) - - op = self.object_designator.insert(session) - action.object_id = op.id - - session.add(action) - session.commit() - - return action - def __init__(self, object_designator_description: ObjectPart, arms: List[str], resolver=None): """ @@ -850,14 +371,14 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[str], self.object_designator_description: ObjectPart = object_designator_description self.arms: List[str] = arms - def ground(self) -> Action: + def ground(self) -> CloseActionPerformable: """ Default resolver that returns a performable designator with the resolved object designator and the first entry from the list of possible arms. :return: A performable designator """ - return self.Action(self.object_designator_description.resolve(), self.arms[0]) + return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) class GraspingAction(ActionDesignatorDescription): @@ -865,51 +386,6 @@ class GraspingAction(ActionDesignatorDescription): Grasps an object described by the given Object Designator description """ - @dataclasses.dataclass - class Action(ActionDesignatorDescription.Action): - arm: str - """ - The arm that should be used to grasp - """ - object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] - """ - Object Designator for the object that should be grasped - """ - - def perform(self) -> Any: - if isinstance(self.object_desig, ObjectPart.Object): - object_pose = self.object_desig.part_pose - else: - object_pose = self.object_desig.bullet_world_object.get_pose() - lt = LocalTransformer() - gripper_name = robot_description.get_tool_frame(self.arm) - - object_pose_in_gripper = lt.transform_pose(object_pose, - BulletWorld.robot.get_link_tf_frame(gripper_name)) - - pre_grasp = object_pose_in_gripper.copy() - pre_grasp.pose.position.x -= 0.1 - - MoveTCPMotion(pre_grasp, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() - - MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).resolve().perform() - MoveGripperMotion("close", self.arm, allow_gripper_collision=True).resolve().perform() - - def to_sql(self) -> ORMGraspingAction: - return ORMGraspingAction(self.arm) - - def insert(self, session: sqlalchemy.orm.session.Session, *args, **kwargs) -> ORMGraspingAction: - action = super().insert(session) - - od = self.object_desig.insert(session) - action.object_id = od.id - - session.add(action) - session.commit() - - return action - def __init__(self, arms: List[str], object_description: Union[ObjectDesignatorDescription, ObjectPart], resolver: Callable = None): """ @@ -924,11 +400,11 @@ def __init__(self, arms: List[str], object_description: Union[ObjectDesignatorDe self.arms: List[str] = arms self.object_description: ObjectDesignatorDescription = object_description - def ground(self) -> Action: + def ground(self) -> GraspingActionPerformable: """ Default resolver that takes the first element from the list of arms and the first solution for the object designator description ond returns it. :return: A performable action designator that contains specific arguments """ - return self.Action(self.arms[0], self.object_description.resolve()) + return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py new file mode 100644 index 000000000..add4bc650 --- /dev/null +++ b/src/pycram/designators/actions/actions.py @@ -0,0 +1,578 @@ +import abc +from typing import Union +from pycram.designator import ActionDesignatorDescription +from pycram.designators.motion_designator import * +from pycram.enums import Arms +from pycram.task import with_tree +from dataclasses import dataclass +from ..location_designator import CostmapLocation +from ..object_designator import BelieveObject +from ...helper import multiply_quaternions +from ...local_transformer import LocalTransformer +from ...orm.base import Base +from ...plan_failures import ObjectUnfetchable, ReachabilityFailure +from ...robot_descriptions import robot_description +from ...orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, + PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, + MoveTorsoAction as ORMMoveTorsoAction, SetGripperAction as ORMSetGripperAction, + LookAtAction as ORMLookAtAction, DetectAction as ORMDetectAction, + TransportAction as ORMTransportAction, OpenAction as ORMOpenAction, + CloseAction as ORMCloseAction, GraspingAction as ORMGraspingAction, Action) + + +@dataclass +class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): + """Base class for performable actions.""" + + @abc.abstractmethod + def perform(self) -> None: + """Perform the action.""" + pass + + @abc.abstractmethod + def to_sql(self) -> Action: + """Convert this action to its ORM equivalent.""" + pass + + @abc.abstractmethod + def insert(self, session: Session, **kwargs) -> Action: + """Insert this action into the database.""" + action = super().insert(session) + return action + + +@dataclass +class ParkArmsActionPerformable(ActionAbstract): + + arm: Arms + """ + Entry from the enum for which arm should be parked + """ + + @with_tree + def perform(self) -> None: + # create the keyword arguments + kwargs = dict() + + # add park left arm if wanted + if self.arm in [Arms.LEFT, Arms.BOTH]: + kwargs["left_arm_config"] = "park" + + # add park right arm if wanted + if self.arm in [Arms.RIGHT, Arms.BOTH]: + kwargs["right_arm_config"] = "park" + MoveArmJointsMotion(**kwargs).resolve().perform() + + def to_sql(self) -> ORMParkArmsAction: + return ORMParkArmsAction(self.arm.name) + + def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class MoveTorsoActionPerformable(ActionAbstract): + """ + Performable Move Torso Action designator. + """ + + position: float + """ + Target position of the torso joint + """ + + @with_tree + def perform(self) -> None: + MoveJointsMotion([robot_description.torso_joint], [self.position]).resolve().perform() + + def to_sql(self) -> ORMMoveTorsoAction: + return ORMMoveTorsoAction(self.position) + + def insert(self, session: Session, **kwargs) -> ORMMoveTorsoAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class SetGripperActionPerformable(ActionAbstract): + + gripper: str + """ + The gripper that should be set + """ + motion: str + """ + The motion that should be set on the gripper + """ + + @with_tree + def perform(self) -> None: + MoveGripperMotion(gripper=self.gripper, motion=self.motion).resolve().perform() + + def to_sql(self) -> ORMSetGripperAction: + return ORMSetGripperAction(self.gripper, self.motion) + + def insert(self, session: Session, *args, **kwargs) -> ORMSetGripperAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + +@dataclass +class ReleaseActionPerformable(ActionAbstract): + + gripper: str + + object_designator: ObjectDesignatorDescription.Object + + def perform(self) -> None: + raise NotImplementedError + + def to_sql(self) -> ORMParkArmsAction: + raise NotImplementedError + + def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: + raise NotImplementedError + + +@dataclass +class GripActionPerformable(ActionAbstract): + + gripper: str + object_designator: ObjectDesignatorDescription.Object + effort: float + + @with_tree + def perform(self) -> None: + raise NotImplementedError() + + def to_sql(self) -> Base: + raise NotImplementedError() + + def insert(self, session: Session, *args, **kwargs) -> Base: + raise NotImplementedError() + + +@dataclass +class PickUpActionPerformable(ActionAbstract): + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be picked up + """ + + arm: str + """ + The arm that should be used for pick up + """ + + grasp: str + """ + The grasp that should be used. For example, 'left' or 'right' + """ + + object_at_execution: Optional[ObjectDesignatorDescription.Object] = dataclasses.field(init=False) + """ + The object at the time this Action got created. It is used to be a static, information holding entity. It is + not updated when the BulletWorld object is changed. + """ + + @with_tree + def perform(self) -> None: + # Store the object's data copy at execution + self.object_at_execution = self.object_designator.frozen_copy() + robot = BulletWorld.robot + # Retrieve object and robot from designators + object = self.object_designator.bullet_world_object + # Get grasp orientation and target pose + grasp = robot_description.grasps.get_orientation_for_grasp(self.grasp) + # oTm = Object Pose in Frame map + oTm = object.get_pose() + # Transform the object pose to the object frame, basically the origin of the object frame + mTo = object.local_transformer.transform_to_object_frame(oTm, object) + # Adjust the pose according to the special knowledge of the object designator + adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) + # Transform the adjusted pose to the map frame + adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") + # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper + ori = multiply_quaternions([adjusted_oTm.orientation.x, adjusted_oTm.orientation.y, + adjusted_oTm.orientation.z, adjusted_oTm.orientation.w], + grasp) + + # Set the orientation of the object pose by grasp in MAP + adjusted_oTm.orientation.x = ori[0] + adjusted_oTm.orientation.y = ori[1] + adjusted_oTm.orientation.z = ori[2] + adjusted_oTm.orientation.w = ori[3] + + # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh + # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" + gripper_frame = robot.get_link_tf_frame(robot_description.get_tool_frame(self.arm)) + # First rotate the gripper, so the further calculations makes sense + tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + tmp_for_rotate_pose.pose.position.x = 0 + tmp_for_rotate_pose.pose.position.y = 0 + tmp_for_rotate_pose.pose.position.z = -0.1 + gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") + + #Perform Gripper Rotate + # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) + # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() + + oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented + prepose = object.local_transformer.transform_pose(oTg, "map") + + # Perform the motion with the prepose and open gripper + BulletWorld.current_bullet_world.add_vis_axis(prepose) + MoveTCPMotion(prepose, self.arm).resolve().perform() + MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() + + # Perform the motion with the adjusted pose -> actual grasp and close gripper + BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() + adjusted_oTm.pose.position.z += 0.03 + MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() + tool_frame = robot_description.get_tool_frame(self.arm) + robot.attach(object, tool_frame) + + # Lift object + BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() + + # Remove the vis axis from the world + BulletWorld.current_bullet_world.remove_vis_axis() + + def to_sql(self) -> ORMPickUpAction: + return ORMPickUpAction(self.arm, self.grasp) + + def insert(self, session: Session, **kwargs) -> ORMPickUpAction: + action = super().insert(session) + + od = self.object_at_execution.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class PlaceActionPerformable(ActionAbstract): + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be place + """ + arm: str + """ + Arm that is currently holding the object + """ + target_location: Pose + """ + Pose in the world at which the object should be placed + """ + + @with_tree + def perform(self) -> None: + object_pose = self.object_designator.bullet_world_object.get_pose() + local_tf = LocalTransformer() + + # Transformations such that the target position is the position of the object and not the tcp + tcp_to_object = local_tf.transform_pose(object_pose, + BulletWorld.robot.get_link_tf_frame( + robot_description.get_tool_frame(self.arm))) + target_diff = self.target_location.to_transform("target").inverse_times( + tcp_to_object.to_transform("object")).to_pose() + + MoveTCPMotion(target_diff, self.arm).resolve().perform() + MoveGripperMotion("open", self.arm).resolve().perform() + BulletWorld.robot.detach(self.object_designator.bullet_world_object) + retract_pose = target_diff + retract_pose.position.x -= 0.07 + MoveTCPMotion(retract_pose, self.arm).resolve().perform() + + def to_sql(self) -> ORMPlaceAction: + return ORMPlaceAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMPlaceAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class NavigateActionPerformable(ActionAbstract): + + target_location: Pose + """ + Location to which the robot should be navigated + """ + + @with_tree + def perform(self) -> None: + MoveMotion(self.target_location).resolve().perform() + + def to_sql(self) -> ORMNavigateAction: + return ORMNavigateAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMNavigateAction: + action = super().insert(session) + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class TransportActionPerformable(ActionAbstract): + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be transported. + """ + arm: str + """ + Arm that should be used + """ + target_location: Pose + """ + Target Location to which the object should be transported + """ + + @with_tree + def perform(self) -> None: + robot_desig = BelieveObject(names=[robot_description.name]) + # ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), + reachable_arm=self.arm) + # Tries to find a pick-up posotion for the robot that uses the given arm + pickup_pose = None + for pose in pickup_loc: + if self.arm in pose.reachable_arms: + pickup_pose = pose + break + if not pickup_pose: + raise ObjectUnfetchable( + f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") + + NavigateActionPerformable(pickup_pose.pose).perform() + PickUpActionPerformable(self.object_designator, self.arm, "front").perform() + # ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + try: + place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), + reachable_arm=self.arm).resolve() + except StopIteration: + raise ReachabilityFailure( + f"No location found from where the robot can reach the target location: {self.target_location}") + NavigateActionPerformable(place_loc.pose).perform() + PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + + def to_sql(self) -> ORMTransportAction: + return ORMTransportAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMTransportAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + pose = self.target_location.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class LookAtActionPerformable(ActionAbstract): + + target: Pose + """ + Position at which the robot should look, given as 6D pose + """ + + @with_tree + def perform(self) -> None: + LookingMotion(target=self.target).resolve().perform() + + def to_sql(self) -> ORMLookAtAction: + return ORMLookAtAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMLookAtAction: + action = super().insert(session) + + pose = self.target.insert(session) + action.pose_id = pose.id + + session.add(action) + session.commit() + return action + + +@dataclass +class DetectActionPerformable(ActionAbstract): + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator loosely describing the object, e.g. only type. + """ + + @with_tree + def perform(self) -> None: + return DetectingMotion(object_type=self.object_designator.type).resolve().perform() + + def to_sql(self) -> ORMDetectAction: + return ORMDetectAction() + + def insert(self, session: Session, *args, **kwargs) -> ORMDetectAction: + action = super().insert(session) + + od = self.object_designator.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class OpenActionPerformable(ActionAbstract): + + object_designator: ObjectPart.Object + """ + Object designator describing the object that should be opened + """ + arm: str + """ + Arm that should be used for opening the container + """ + + @with_tree + def perform(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + OpeningMotion(self.object_designator, self.arm).resolve().perform() + + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() + + def to_sql(self) -> ORMOpenAction: + return ORMOpenAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMOpenAction: + action = super().insert(session) + + op = self.object_designator.insert(session) + action.object_id = op.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class CloseActionPerformable(ActionAbstract): + + object_designator: ObjectPart.Object + """ + Object designator describing the object that should be closed + """ + arm: str + """ + Arm that should be used for closing + """ + + @with_tree + def perform(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + ClosingMotion(self.object_designator, self.arm).resolve().perform() + + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() + + def to_sql(self) -> ORMCloseAction: + return ORMCloseAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMCloseAction: + action = super().insert(session) + + op = self.object_designator.insert(session) + action.object_id = op.id + + session.add(action) + session.commit() + + return action + + +@dataclass +class GraspingActionPerformable(ActionAbstract): + + arm: str + """ + The arm that should be used to grasp + """ + object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] + """ + Object Designator for the object that should be grasped + """ + + @with_tree + def perform(self) -> None: + if isinstance(self.object_desig, ObjectPart.Object): + object_pose = self.object_desig.part_pose + else: + object_pose = self.object_desig.bullet_world_object.get_pose() + lt = LocalTransformer() + gripper_name = robot_description.get_tool_frame(self.arm) + + object_pose_in_gripper = lt.transform_pose(object_pose, + BulletWorld.robot.get_link_tf_frame(gripper_name)) + + pre_grasp = object_pose_in_gripper.copy() + pre_grasp.pose.position.x -= 0.1 + + MoveTCPMotion(pre_grasp, self.arm).resolve().perform() + MoveGripperMotion("open", self.arm).resolve().perform() + + MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).resolve().perform() + MoveGripperMotion("close", self.arm, allow_gripper_collision=True).resolve().perform() + + def to_sql(self) -> ORMGraspingAction: + return ORMGraspingAction(self.arm) + + def insert(self, session: Session, *args, **kwargs) -> ORMGraspingAction: + action = super().insert(session) + + od = self.object_desig.insert(session) + action.object_id = od.id + + session.add(action) + session.commit() + + return action diff --git a/src/pycram/task.py b/src/pycram/task.py index f17e2b7e1..1c404577f 100644 --- a/src/pycram/task.py +++ b/src/pycram/task.py @@ -56,9 +56,13 @@ def execute(self) -> Any: # self.kwargs.items()])) def __str__(self) -> str: - return "%s(%s)" % ( - self.function.__name__, ", ".join(["%s, " % (str(value.__class__).split(".")[-2]) for value in - self.kwargs.values()])) + if "self" in self.kwargs: + class_name = self.kwargs["self"].__class__.__name__ + else: + class_name = "" + function_name = self.function.__name__ + + return f"{function_name}({class_name})" def __eq__(self, other): return isinstance(other, Code) and other.function.__name__ == self.function.__name__ \ diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 49c58c436..8711b3f8b 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -1,6 +1,8 @@ import time import unittest from pycram.designators import action_designator, object_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.robot_descriptions import robot_description from pycram.process_module import simulated_robot from pycram.pose import Pose @@ -61,8 +63,8 @@ def test_pick_up(self): description = action_designator.PickUpAction(object_description, ["left"], ["front"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() description.resolve().perform() self.assertTrue(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) @@ -71,9 +73,9 @@ def test_place(self): description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() self.assertFalse(object_description.resolve().bullet_world_object in self.robot.attachments.keys()) diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index a9103bd0a..4c7d10822 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -6,6 +6,8 @@ from pycram import task from pycram.bullet_world import BulletWorld, Object from pycram.designators import action_designator, object_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.orm.base import Base from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot @@ -68,9 +70,9 @@ def plan(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() def test_costmap_no_obstacles(self): @@ -85,7 +87,7 @@ def test_costmap_no_obstacles(self): with simulated_robot: # action_designator.NavigateAction.Action(sample.pose).perform() action_designator.MoveTorsoAction.Action(sample.torso_height).perform() - action_designator.PickUpAction.Action( + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() @@ -98,10 +100,10 @@ def test_costmap_with_obstacles(self): for i in range(20): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() try: - action_designator.PickUpAction.Action( + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() except pycram.plan_failures.PlanFailure: diff --git a/test/test_jpt_resolver.py b/test/test_jpt_resolver.py index 98884a0d7..9979d4a62 100644 --- a/test/test_jpt_resolver.py +++ b/test/test_jpt_resolver.py @@ -7,6 +7,8 @@ import pycram.plan_failures from pycram.bullet_world import BulletWorld, Object from pycram.designators import action_designator, object_designator +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.robot_descriptions import robot_description @@ -63,9 +65,9 @@ def test_costmap_no_obstacles(self): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() - action_designator.PickUpAction.Action( + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() @@ -77,10 +79,10 @@ def test_costmap_with_obstacles(self): for i in range(20): sample = next(iter(cml)) with simulated_robot: - action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() + NavigateActionPerformable(sample.pose).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() try: - action_designator.PickUpAction.Action( + PickUpActionPerformable( object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() except pycram.plan_failures.PlanFailure: diff --git a/test/test_task_tree.py b/test/test_task_tree.py index abe3f3ab8..f343d7b9a 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -1,3 +1,5 @@ +from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ + NavigateActionPerformable from pycram.pose import Pose from pycram.process_module import simulated_robot import pycram.task @@ -17,9 +19,9 @@ def plan(self): description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() def setUp(self): From 8d11a5d693a6f44524e27a88acdb646bc586cc6c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 5 Jan 2024 13:09:42 +0100 Subject: [PATCH 02/12] [orm] fixed orm querying --- examples/intro.ipynb | 8521 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 8167 insertions(+), 354 deletions(-) diff --git a/examples/intro.ipynb b/examples/intro.ipynb index aea7c14fb..74a7d5342 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -12,11 +12,25 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2023-04-05T16:37:51.973404Z", - "start_time": "2023-04-05T16:37:51.483749Z" + "end_time": "2024-01-05T11:38:17.693331699Z", + "start_time": "2024-01-05T11:38:17.084580161Z" } }, - "outputs": [], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "pybullet build time: May 20 2022 19:44:17\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1704454697.683303]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "[WARN] [1704454697.688683]: Failed to import Giskard messages\n" + ] + } + ], "source": [ "import pycram" ] @@ -39,8 +53,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2023-04-05T16:37:59.788099Z", - "start_time": "2023-04-05T16:37:57.586879Z" + "end_time": "2024-01-05T11:38:18.005042043Z", + "start_time": "2024-01-05T11:38:17.754902302Z" } }, "outputs": [ @@ -48,8 +62,6 @@ "name": "stderr", "output_type": "stream", "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" ] @@ -63,27 +75,6 @@ "world = BulletWorld()" ] }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The world can also be closed with the 'exit' method" - ] - }, - { - "cell_type": "code", - "execution_count": 37, - "metadata": { - "ExecuteTime": { - "end_time": "2023-04-05T16:38:04.800655Z", - "start_time": "2023-04-05T16:38:03.943911Z" - } - }, - "outputs": [], - "source": [ - "world.exit()" - ] - }, { "cell_type": "markdown", "metadata": {}, @@ -98,86 +89,29 @@ }, { "cell_type": "code", - "execution_count": 4, - "metadata": {}, + "execution_count": 3, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:18.052419130Z", + "start_time": "2024-01-05T11:38:18.021916002Z" + } + }, "outputs": [ { "data": { - "text/plain": [ - "[array([[[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " [[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " [[255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " ...,\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255],\n", - " [255, 255, 255, 255]],\n", - " \n", - " ...,\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]],\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]],\n", - " \n", - " [[239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " ...,\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255],\n", - " [239, 239, 239, 255]]], dtype=uint8),\n", - " array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n", - " 0.99999994],\n", - " ...,\n", - " [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n", - " 0.80473447],\n", - " [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n", - " 0.8031688 ],\n", - " [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n", - " 0.80160314]], dtype=float32),\n", - " array([[-1, -1, -1, ..., -1, -1, -1],\n", - " [-1, -1, -1, ..., -1, -1, -1],\n", - " [-1, -1, -1, ..., -1, -1, -1],\n", - " ...,\n", - " [ 1, 1, 1, ..., 1, 1, 1],\n", - " [ 1, 1, 1, ..., 1, 1, 1],\n", - " [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" - ] + "text/plain": "[array([[[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n ...,\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]]], dtype=uint8),\n array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n ...,\n [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n 0.80473447],\n [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n 0.8031688 ],\n [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n 0.80160314]], dtype=float32),\n array([[-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n ...,\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" }, - "execution_count": 4, + "execution_count": 3, "metadata": {}, "output_type": "execute_result" + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" + ] } ], "source": [ @@ -212,8 +146,13 @@ }, { "cell_type": "code", - "execution_count": 5, - "metadata": {}, + "execution_count": 4, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:18.094930935Z", + "start_time": "2024-01-05T11:38:18.028522514Z" + } + }, "outputs": [], "source": [ "milk = Object(\"Milk\", ObjectType.MILK, \"milk.stl\")" @@ -230,8 +169,13 @@ }, { "cell_type": "code", - "execution_count": 6, - "metadata": {}, + "execution_count": 5, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:18.097339564Z", + "start_time": "2024-01-05T11:38:18.096109982Z" + } + }, "outputs": [], "source": [ "milk.set_position(Pose([1, 0, 0]))" @@ -246,8 +190,13 @@ }, { "cell_type": "code", - "execution_count": 7, - "metadata": {}, + "execution_count": 6, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:18.142227637Z", + "start_time": "2024-01-05T11:38:18.099015288Z" + } + }, "outputs": [], "source": [ "milk.remove()" @@ -262,8 +211,13 @@ }, { "cell_type": "code", - "execution_count": 8, - "metadata": {}, + "execution_count": 7, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:21.716481380Z", + "start_time": "2024-01-05T11:38:18.142151725Z" + } + }, "outputs": [ { "name": "stderr", @@ -304,8 +258,13 @@ }, { "cell_type": "code", - "execution_count": 9, - "metadata": {}, + "execution_count": 8, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:22.274205765Z", + "start_time": "2024-01-05T11:38:21.717433092Z" + } + }, "outputs": [], "source": [ "import pycram.costmaps as cm\n", @@ -314,8 +273,13 @@ }, { "cell_type": "code", - "execution_count": 10, - "metadata": {}, + "execution_count": 9, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:30.651191441Z", + "start_time": "2024-01-05T11:38:30.646880498Z" + } + }, "outputs": [], "source": [ "v.visualize()" @@ -323,8 +287,13 @@ }, { "cell_type": "code", - "execution_count": 11, - "metadata": {}, + "execution_count": 10, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:33.983967866Z", + "start_time": "2024-01-05T11:38:30.649221674Z" + } + }, "outputs": [], "source": [ "v.close_visualization()" @@ -340,8 +309,13 @@ }, { "cell_type": "code", - "execution_count": 12, - "metadata": {}, + "execution_count": 11, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:34.747939331Z", + "start_time": "2024-01-05T11:38:33.985076157Z" + } + }, "outputs": [], "source": [ "o = cm.OccupancyCostmap(0.2, from_ros=False, size=300, resolution=0.02, origin=Pose([0, 0, 0.1], [0, 0, 0, 1]))" @@ -349,8 +323,13 @@ }, { "cell_type": "code", - "execution_count": 13, - "metadata": {}, + "execution_count": 12, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:34.845540496Z", + "start_time": "2024-01-05T11:38:34.749095455Z" + } + }, "outputs": [], "source": [ "s = cm.SemanticCostmap(kitchen, \"kitchen_island_surface\", size=100, resolution=0.02)\n", @@ -368,8 +347,13 @@ }, { "cell_type": "code", - "execution_count": 14, - "metadata": {}, + "execution_count": 13, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:35.719367991Z", + "start_time": "2024-01-05T11:38:34.846943933Z" + } + }, "outputs": [], "source": [ "o.visualize()" @@ -377,8 +361,13 @@ }, { "cell_type": "code", - "execution_count": 15, - "metadata": {}, + "execution_count": 14, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:36.074271101Z", + "start_time": "2024-01-05T11:38:36.067925005Z" + } + }, "outputs": [], "source": [ "o.close_visualization()" @@ -393,8 +382,13 @@ }, { "cell_type": "code", - "execution_count": 16, - "metadata": {}, + "execution_count": 15, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:36.075213164Z", + "start_time": "2024-01-05T11:38:36.070266632Z" + } + }, "outputs": [], "source": [ "ov = o + v" @@ -402,8 +396,13 @@ }, { "cell_type": "code", - "execution_count": 17, - "metadata": {}, + "execution_count": 16, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:38.237916925Z", + "start_time": "2024-01-05T11:38:36.073992561Z" + } + }, "outputs": [], "source": [ "ov.visualize()" @@ -411,8 +410,13 @@ }, { "cell_type": "code", - "execution_count": 18, - "metadata": {}, + "execution_count": 17, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:39.305574111Z", + "start_time": "2024-01-05T11:38:38.238541874Z" + } + }, "outputs": [], "source": [ "ov.close_visualization()" @@ -437,8 +441,13 @@ }, { "cell_type": "code", - "execution_count": 19, - "metadata": {}, + "execution_count": 18, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:41.970422232Z", + "start_time": "2024-01-05T11:38:39.305851886Z" + } + }, "outputs": [ { "name": "stderr", @@ -466,9 +475,13 @@ }, { "cell_type": "code", - "execution_count": 20, + "execution_count": 19, "metadata": { - "scrolled": true + "scrolled": true, + "ExecuteTime": { + "end_time": "2024-01-05T11:38:42.271284276Z", + "start_time": "2024-01-05T11:38:41.989603716Z" + } }, "outputs": [ { @@ -497,8 +510,13 @@ }, { "cell_type": "code", - "execution_count": 21, - "metadata": {}, + "execution_count": 20, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:42.355865055Z", + "start_time": "2024-01-05T11:38:42.270253375Z" + } + }, "outputs": [ { "name": "stdout", @@ -518,8 +536,13 @@ }, { "cell_type": "code", - "execution_count": 22, - "metadata": {}, + "execution_count": 21, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:42.450970248Z", + "start_time": "2024-01-05T11:38:42.358883797Z" + } + }, "outputs": [ { "name": "stdout", @@ -560,8 +583,13 @@ }, { "cell_type": "code", - "execution_count": 23, - "metadata": {}, + "execution_count": 22, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:42.957994018Z", + "start_time": "2024-01-05T11:38:42.494000112Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import *\n", @@ -576,9 +604,13 @@ }, { "cell_type": "code", - "execution_count": 24, + "execution_count": 23, "metadata": { - "scrolled": true + "scrolled": true, + "ExecuteTime": { + "end_time": "2024-01-05T11:38:43.465280115Z", + "start_time": "2024-01-05T11:38:42.966741953Z" + } }, "outputs": [], "source": [ @@ -618,176 +650,19 @@ }, { "cell_type": "code", - "execution_count": 25, - "metadata": {}, + "execution_count": 24, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:43.472245439Z", + "start_time": "2024-01-05T11:38:43.467689247Z" + } + }, "outputs": [ { "data": { - "text/plain": [ - "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448180\n", - " nsecs: 830921888\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448185\n", - " nsecs: 354960680\n", - " frame_id: \"map\"\n", - "child_frame_id: \"Milk_4\"\n", - "transform: \n", - " translation: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': }), _pose=, \n", - "local_transformer=, \n", - "name=Milk, \n", - "type=ObjectType.MILK, \n", - "color=[1, 1, 1, 1], \n", - "id=4, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", - "joints: ..., \n", - "links: ..., \n", - "attachments: ..., \n", - "cids: ..., \n", - "original_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448180\n", - " nsecs: 830921888\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.0\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "tf_frame=Milk_4, \n", - "urdf_object: ..., \n", - "_current_pose=header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448184\n", - " nsecs: 735546827\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448185\n", - " nsecs: 354960680\n", - " frame_id: \"map\"\n", - "child_frame_id: \"Milk_4\"\n", - "transform: \n", - " translation: \n", - " x: 0.6\n", - " y: -0.5\n", - " z: 0.7\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_joint_states={}, \n", - "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': })>)" - ] + "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1704454719\n nsecs: 314873933\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1704454722\n nsecs: 356852293\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1704454719\n nsecs: 314873933\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1704454722\n nsecs: 356852293\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...)>)" }, - "execution_count": 25, + "execution_count": 24, "metadata": {}, "output_type": "execute_result" } @@ -809,11 +684,7878 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 25, "metadata": { - "scrolled": false + "scrolled": false, + "ExecuteTime": { + "end_time": "2024-01-05T11:38:50.957195391Z", + "start_time": "2024-01-05T11:38:43.471315489Z" + } }, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Resolved: CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 189191341\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -1.26\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.443467406158451\n", + " w: 0.8962904996010476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 901956796\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -1.26\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.443467406158451\n", + " w: 0.8962904996010476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 911677598\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.08868143268496492\n", + " w: -0.9960600401064899, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 921111106\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.1341778331057823\n", + " w: -0.9909572690600926, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 930423736\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.1018321670517163\n", + " w: -0.9948015931599383, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 939850091\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.1181477706660459\n", + " w: -0.9929960243055576, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 949112415\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7414525335518785\n", + " w: -0.6710053207609464, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 958681583\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7421299374143975\n", + " w: -0.6702560376403205, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 968044519\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.07625058147116927\n", + " w: -0.997088686539622, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 977337121\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.0637115975084186\n", + " w: -0.9979683523754275, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454724\n", + " nsecs: 986764192\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.08526395561915938\n", + " w: -0.9963583983046332, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 5548715\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.025615781497071395\n", + " w: -0.9996718620318842, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 24160146\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.44000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7435677691352427\n", + " w: -0.6686605810897175, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 33551692\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.42000000000000004\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7443316400531483\n", + " w: -0.6678101598626592, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 43041944\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7451280178851306\n", + " w: -0.6669214623646299, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 52456855\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.0684794522339555\n", + " w: -0.9976525269961167, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 61823606\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.44\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.051517987991811\n", + " w: -0.9986720667532839, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 71311473\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.03442144373073932\n", + " w: -0.9994074065222308, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 80672740\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.051081118766858044\n", + " w: -0.9986945074974259, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 90170145\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 118748664\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 128309965\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.012817353286048666\n", + " w: 0.9999178543534167, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 137932062\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.54\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.025615781497071274\n", + " w: 0.9996718620318842, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 147601604\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.56\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.03837651950358723\n", + " w: 0.9992633500488202, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 157281398\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.58\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.051081118766857926\n", + " w: 0.9986945074974259, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 166773319\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.6\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.06371159750841848\n", + " w: 0.9979683523754275, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 176612615\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.18000000000000005\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.07625058147116914\n", + " w: 0.997088686539622, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 186205387\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6400579972438699\n", + " w: -0.7683265973296552, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 196010589\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.45999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6386358371363977\n", + " w: -0.7695091081495349, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 205993652\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.44000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.637152936590636\n", + " w: -0.7707373971684058, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 215711593\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.42000000000000004\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6356053782081866\n", + " w: -0.7720141211097294, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 225684404\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6339889056055383\n", + " w: -0.7733421413379021, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 235526561\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6322988865446836\n", + " w: -0.7747245433535415, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 255168437\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.21999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6154122094026357\n", + " w: -0.7882054380161091, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 264625310\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.20000000000000007\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6127638044913316\n", + " w: -0.7902661070204827, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 274528026\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.017233697316775633\n", + " w: -0.9998514888106103, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 293281793\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.01723369731677551\n", + " w: 0.9998514888106103, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 302706003\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3745654411331824\n", + " w: 0.9272004801059501, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 321676969\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.21999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7541778209662121\n", + " w: -0.6566702478129005, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 331218719\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.20000000000000007\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7554539549957066\n", + " w: -0.6552017413601289, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 341029405\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.18000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7568004989740795\n", + " w: -0.6536459322542933, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 350405693\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.16000000000000003\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7582233771877503\n", + " w: -0.6519948698310459, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 360239028\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.14\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7597291886829179\n", + " w: -0.6502396172667391, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 369774341\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.12\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7613253045921371\n", + " w: -0.6483700953835624, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 379291534\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.09999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7630199824727258\n", + " w: -0.6463748961301956, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 408546209\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.040000000000000036\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7687942703292956\n", + " w: -0.6394961844365031, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 418382167\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7785979849482799\n", + " w: -0.6275230496439776, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 428440093\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.54\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.034421443730739416\n", + " w: 0.9994074065222308, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 448060989\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.58\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.06847945223395538\n", + " w: 0.9976525269961167, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 458268404\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.6\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.08526395561915948\n", + " w: 0.9963583983046332, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 468119144\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.10183216705171617\n", + " w: 0.9948015931599383, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 477586746\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.118147770666046\n", + " w: 0.9929960243055576, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 487447023\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.13417783310578218\n", + " w: 0.9909572690600926, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 496778726\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.6799999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1498930671991917\n", + " w: 0.9887022142210559, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 515697717\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.18028099412059023\n", + " w: 0.98361514992343, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 535202741\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.76\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.20915386140954798\n", + " w: 0.9778827446363269, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 562966585\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.8400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2620133869693895\n", + " w: 0.9650642388197943, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 603397607\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.30826764220152825\n", + " w: 0.9512996692796181, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 613683938\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.318832815984807\n", + " w: 0.947810970315916, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 623685836\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.32901544105897695\n", + " w: 0.9443245414288284, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 635320425\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3388256770636711\n", + " w: 0.9408491699323249, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 645336151\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.34827434473373386\n", + " w: 0.9373926502807073, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 665030956\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.36613250713959444\n", + " w: 0.9305627261048418, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 684486150\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: 0.09999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8022929282893952\n", + " w: -0.5969305296404493, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 694182634\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.636848728643575\n", + " w: 0.7709887786635173, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 703764915\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6394961844365034\n", + " w: 0.7687942703292954, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 722873687\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6275230496439778\n", + " w: 0.7785979849482799, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 732309579\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6238505014869475\n", + " w: 0.7815436979430415, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 742166519\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6198304093435398\n", + " w: 0.7847357922594203, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 752015113\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6419537995511603\n", + " w: 0.7667433203111904, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 762117147\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.26\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4586899104028806\n", + " w: 0.8885964022516619, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 772179365\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4068385849311433\n", + " w: 0.9135000633887361, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 781810283\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.08\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.39911243533693425\n", + " w: 0.9169019925594128, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 792211532\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.39106548646982875\n", + " w: 0.9203628552327153, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 802207708\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3826834323650898\n", + " w: 0.9238795325112867, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 811917543\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3739517258340434\n", + " w: 0.9274481693042154, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 822474241\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: 0.07999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5435734062236173\n", + " w: -0.8393616336517022, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 832451105\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.36485567419311876\n", + " w: 0.9310640885616225, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 842451095\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7398679249122764\n", + " w: 0.6727521487784355, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 864063024\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6908662457673519\n", + " w: -0.7229825934691131, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 874287605\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7370989134318549\n", + " w: 0.6757848709593749, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 884132862\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5039557239143624\n", + " w: -0.8637294879381802, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 893823862\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.49561612558176465\n", + " w: -0.8685416835496846, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 913730621\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6928318560989847\n", + " w: -0.7210991742988171, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 923520088\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6933854908702646\n", + " w: -0.7205668331602574, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 932972908\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3553805575128866\n", + " w: 0.9347217015464174, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 942650079\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.10682611122099109\n", + " w: -0.9942777187292293, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 952360153\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4866443184625216\n", + " w: 0.8736001987798239, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 962044715\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.0859890841337593\n", + " w: -0.9962960791902362, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454725\n", + " nsecs: 972100019\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5039557239143623\n", + " w: 0.8637294879381803, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 10155916\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6908662457673519\n", + " w: 0.7229825934691132, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 19874572\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6915789093529722\n", + " w: 0.7223009152272711, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 39346933\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.44\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.06480582077938048\n", + " w: -0.9978978933704143, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 48823833\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.043355575214250826\n", + " w: -0.9990597049715505, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 58706760\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6938978092954753\n", + " w: 0.7200734894821085, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 68709850\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.58\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6943732675901296\n", + " w: 0.7196150118335541, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 87986469\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 97659826\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.02172373868536963\n", + " w: 0.9997640117435364, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 107616424\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.33999999999999997\n", + " y: -1.12\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5537478323882296\n", + " w: 0.8326844168863359, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 117786407\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7967527613758144\n", + " w: -0.6043054171857262, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 127777814\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8000000000000002\n", + " w: -0.5999999999999999, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 138854026\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.54\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.04335557521425048\n", + " w: 0.9990597049715505, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 149201869\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.56\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.06480582077938013\n", + " w: 0.9978978933704143, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 159495592\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8112421851755609\n", + " w: 0.584710284663765, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 169792652\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8072185755038553\n", + " w: 0.5902526335066423, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 179657697\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8034804340438839\n", + " w: 0.5953311617147653, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 189663410\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.58\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.08598908413375941\n", + " w: 0.9962960791902362, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 199556350\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.6\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.10682611122099096\n", + " w: 0.9942777187292293, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 209797859\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6043054171857263\n", + " w: -0.7967527613758143, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 219633102\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6\n", + " w: -0.8, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 229408979\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7384225572267273\n", + " w: -0.6743382882342812, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 239880800\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7937170381968226\n", + " w: 0.6082871552778866, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 250423669\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.74\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.790873617837808\n", + " w: 0.6119795099577574, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 260511398\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7347602405829027\n", + " w: -0.6783269041240771, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 280463457\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.12724528989938316\n", + " w: 0.9918712800552408, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 291324138\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1471837619629651\n", + " w: 0.9891091649633165, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 313117504\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7327590618734483\n", + " w: 0.680488175681506, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 323770761\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.64\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7318630507095947\n", + " w: 0.6814517407755631, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 334040164\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.33523500300737585\n", + " w: 0.9421345406886665, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 344825744\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.74\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.23813352749512762\n", + " w: 0.9712324248513985, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 355269432\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.76\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2543984682342504\n", + " w: 0.9670994878294927, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 365238666\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.78\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2700013373907261\n", + " w: 0.9628599471404028, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 376126766\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.8\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.28494683992052433\n", + " w: 0.9585433210968125, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 387210607\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.13999999999999996\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2992447144182441\n", + " w: 0.9541763992536934, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 408796787\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.9\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.31340294084074444\n", + " w: 0.9496202381333145, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 419713258\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.88\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3018224483858295\n", + " w: 0.9533641537473408, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 439749002\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.8400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.27727888479854484\n", + " w: 0.9607894774844672, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 449327468\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.26429951415565045\n", + " w: 0.9644406497120946, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 470635890\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.8\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2508413082792208\n", + " w: 0.9680282217274292, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 480293035\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6308905395898716\n", + " w: -0.7758718496349771, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 500214099\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6368487286435749\n", + " w: -0.7709887786635174, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 520314931\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.45999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6308905395898716\n", + " w: -0.7758718496349771, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 530125856\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.44000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6292425428779993\n", + " w: -0.7772089952081288, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 540076732\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.8400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.323044113557137\n", + " w: 0.9463839076696536, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 549759149\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.30924417189076625\n", + " w: 0.9509826718461247, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 569632768\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.78\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.27958765375807687\n", + " w: 0.9601201715754407, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 589303016\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6257273935913882\n", + " w: -0.7800418122827314, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 599044799\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6238505014869477\n", + " w: -0.7815436979430415, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 609395503\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.7\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2116996959579716\n", + " w: 0.9773347628787704, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 619220018\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.6799999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.19294175778498965\n", + " w: 0.9812102109654376, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 629041433\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.17350299206578976\n", + " w: 0.9848333421164307, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 639055252\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.15341808887275654\n", + " w: 0.988161368404286, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 648601055\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.78\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.23690237287865074\n", + " w: 0.9715334609391818, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 668470382\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.58\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.08980559531591699\n", + " w: 0.9959593139531121, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 678457498\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.56\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.06771200763417459\n", + " w: 0.9977049082880918, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 688319921\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.54\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.0453144211719414\n", + " w: 0.9989727740203193, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 697925806\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.022709687246860417\n", + " w: 0.9997421017968333, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 708099126\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 717890977\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.022709687246860538\n", + " w: -0.9997421017968333, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 727897167\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.045314421171941524\n", + " w: -0.9989727740203193, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 737935304\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.44\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.06771200763417472\n", + " w: -0.9977049082880918, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 747606992\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.08980559531591711\n", + " w: -0.9959593139531121, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 761164903\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.11150592856108661\n", + " w: -0.9937637686571844, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 772180557\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.1327331510254085\n", + " w: -0.9911518100769762, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 786565303\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.15341808887275665\n", + " w: -0.9881613684042859, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 800817966\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.17350299206579006\n", + " w: -0.9848333421164306, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 811119318\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.76\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.22248407835269934\n", + " w: 0.9749363234999248, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 821317911\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.74\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.20759148751784465\n", + " w: 0.978215607271796, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 831474781\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.19223376424350164\n", + " w: 0.9813491630835448, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 851423263\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.6799999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1601822430069672\n", + " w: 0.9870874576374967, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 861536979\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.24\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6079025311911547\n", + " w: -0.7940116577049655, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 871807813\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.21999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6051264893449574\n", + " w: -0.7961293436955124, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 891721963\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.18000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5990966850218961\n", + " w: -0.8006766900539662, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 901814699\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.16000000000000003\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.595815661390807\n", + " w: -0.8031212222581565, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 915273189\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.14\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5923364782216495\n", + " w: -0.805690695346529, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 926554679\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.12\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5886413254594184\n", + " w: -0.8083943282590367, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 940182209\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.09999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5847102846637651\n", + " w: -0.8112421851755608, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 965167760\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.06000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5760484367663208\n", + " w: -0.8174155604703632, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 975308895\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.040000000000000036\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5712642314097769\n", + " w: -0.8207662139195283, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454726\n", + " nsecs: 985270500\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5661364326251805\n", + " w: -0.8243115549684078, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 14838933\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5483036971357891\n", + " w: -0.8362792928843958, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 24997949\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.541385747733913\n", + " w: -0.840774328907937, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 34455060\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.14353028825236291\n", + " w: 0.9896459247398505, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 44534921\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.12649723679630143\n", + " w: 0.9919669596730026, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 54246187\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.10911677189150902\n", + " w: 0.9940289382568178, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 83569049\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.31999999999999995\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.4812091581666131\n", + " w: -0.8766058099833582, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 93582630\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.56\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.05530038574178813\n", + " w: 0.9984697628555457, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 112976551\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.01850900096213863\n", + " w: 0.9998286937687794, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 123248338\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 132923603\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.36\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.532573437384568\n", + " w: 0.8463837981627399, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 142979860\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.36\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5257311121191336\n", + " w: 0.85065080835204, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 152651309\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.018509000962138755\n", + " w: -0.9998286937687794, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 163275718\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.036961098084952626\n", + " w: -0.9993167051682638, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 173099279\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.36\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5019268181932334\n", + " w: 0.8649100931185952, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 199396848\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.31999999999999995\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.49806072645607874\n", + " w: 0.8671421525690256, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 220230817\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: -0.020000000000000018\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.16966474960750313\n", + " w: 0.9855018380199112, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 230001449\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.07347291217220556\n", + " w: -0.9972972130598458, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 240165948\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.36\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5104644303570166\n", + " w: -0.8598988692516618, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 249710559\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.36\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5183792016009997\n", + " w: -0.8551508658403557, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 260489702\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.09142755332923423\n", + " w: -0.9958117304451831, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 270841121\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.10911677189150912\n", + " w: -0.9940289382568177, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 280830144\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.12649723679630132\n", + " w: -0.9919669596730026, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 290997505\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.14353028825236303\n", + " w: -0.9896459247398504, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 300702810\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: -0.32\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.16018224300696732\n", + " w: -0.9870874576374967, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 310925006\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.05999999999999994\n", + " y: 0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.47918806782228074\n", + " w: -0.8777122510576854, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 320628404\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.46\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7630199824727257\n", + " w: 0.6463748961301957, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 330796957\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.44\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.764133282892573\n", + " w: 0.6450583895864149, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 350512981\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.3\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7733421413379024\n", + " w: 0.6339889056055382, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 360772371\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.28\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7749013304032105\n", + " w: 0.6320822162815011, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 370627880\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.26\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7765341206443674\n", + " w: 0.6300752014443031, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 380810976\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.24\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7782457171509326\n", + " w: 0.6279598743042668, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 390733242\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.22\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7800418122827314\n", + " w: 0.6257273935913882, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 401291370\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.2000000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7819286417257463\n", + " w: 0.6233679485255313, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 411706209\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.1800000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.783913048024316\n", + " w: 0.6208706251202633, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 421663045\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.1600000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7860025526744504\n", + " w: 0.6182232502820061, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 431575298\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.1400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7882054380161092\n", + " w: 0.6154122094026356, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 442079544\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.12\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7905308403287533\n", + " w: 0.6124222321969667, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 451843976\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7929888557099438\n", + " w: 0.6092361403592484, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 471526861\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7983486481160277\n", + " w: 0.602195513144453, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 481531858\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8012765844860855\n", + " w: 0.5982941042282742, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 510531187\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7882054380161092\n", + " w: -0.6154122094026356, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 520304441\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8043897838851272\n", + " w: 0.5941019067308558, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 529991149\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7947066772690753\n", + " w: -0.6069936549124264, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 539878845\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8077053073689017\n", + " w: 0.5895864113496071, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 549521207\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.584710284663765\n", + " w: 0.8112421851755609, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 559333324\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4310817166804798\n", + " w: 0.9023128911546209, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 568988084\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6794495938313833\n", + " w: -0.7337221881900318, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 588930606\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5800385487693184\n", + " w: 0.8145890264063119, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 598807811\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5750132745107971\n", + " w: 0.818144079081656, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 619289398\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8112421851755609\n", + " w: 0.584710284663765, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 629359006\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5637378164623551\n", + " w: 0.8259537967043049, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 639499425\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5847102846637648\n", + " w: 0.8112421851755609, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 649319171\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5902526335066423\n", + " w: 0.8072185755038553, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 659330844\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.88\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.35948867915153393\n", + " w: 0.9331494465314146, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 669612884\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.86\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3469462477349362\n", + " w: 0.9378850149046248, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 679676771\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.8400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.333732774306416\n", + " w: 0.9426677226646422, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 689470529\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3198189174888669\n", + " w: 0.9474786857846721, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 699056148\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5953311617147652\n", + " w: 0.8034804340438839, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 708853006\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5573899686393252\n", + " w: 0.8302508192469624, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 718634128\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5504910087462066\n", + " w: 0.8348410922382677, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 728414297\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.74\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2566679351570243\n", + " w: 0.9664996487646695, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 749159097\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.7\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2203854364245398\n", + " w: 0.9754128661300122, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 759160280\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.6799999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.20106587226819733\n", + " w: 0.9795777228015289, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 769248008\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1809865534660407\n", + " w: 0.9834855705420817, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 779141187\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1601822430069672\n", + " w: 0.9870874576374967, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 789060115\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.13870121188940068\n", + " w: 0.9903342737785114, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 818681716\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.56\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.07088902009067935\n", + " w: 0.9974842088126424, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 829430341\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.54\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.04745802042883704\n", + " w: 0.9988732333469429, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 839163064\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.023789307215243066\n", + " w: 0.9997169943850204, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 848954200\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.5\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 1.2246467991473532e-16\n", + " w: -1.0, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 859354734\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.02378930721524297\n", + " w: -0.9997169943850204, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 879795551\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.44\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.07088902009067946\n", + " w: -0.9974842088126424, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 899379730\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.11660571451398305\n", + " w: -0.9931782857788845, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 909643888\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.13870121188940082\n", + " w: -0.9903342737785114, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 929649591\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.1809865534660408\n", + " w: -0.9834855705420817, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 939815521\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.18\n", + " y: -0.32\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.20106587226819744\n", + " w: -0.9795777228015289, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 959586143\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6082871552778866\n", + " w: 0.7937170381968226, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 969409227\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.45999999999999996\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6119795099577574\n", + " w: 0.790873617837808, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 979031085\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8150216638044884\n", + " w: 0.5794304855022417, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 988930702\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.12\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.16018224300696732\n", + " w: -0.9870874576374967, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454727\n", + " nsecs: 999084234\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8190674767950149\n", + " w: 0.5736971923032635, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 8918762\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8234061353888457\n", + " w: 0.5674524968700076, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 28315067\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5413857477339129\n", + " w: 0.8407743289079371, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 37947177\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6727521487784355\n", + " w: 0.7398679249122764, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 47890901\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6743382882342814\n", + " w: 0.7384225572267273, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 57522058\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6757848709593749\n", + " w: 0.7370989134318549, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 67149162\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6771094889847062\n", + " w: 0.7358822867326472, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 76910734\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5483036971357889\n", + " w: 0.8362792928843958, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 87213277\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.554700196225229\n", + " w: 0.8320502943378437, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 97060918\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6804881756815059\n", + " w: 0.7327590618734483, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 108145713\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.5599999999999999\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6814517407755631\n", + " w: 0.7318630507095948, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 118573904\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5606288093051838\n", + " w: 0.8280672304692729, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 128924608\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5661364326251803\n", + " w: 0.8243115549684079, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 139441013\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5712642314097768\n", + " w: 0.8207662139195284, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 149400472\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5760484367663208\n", + " w: 0.8174155604703632, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 159833192\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.08\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5805210231682381\n", + " w: 0.8142452589114058, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 169833660\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.584710284663765\n", + " w: 0.8112421851755609, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 179687261\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.12\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5886413254594183\n", + " w: 0.8083943282590367, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 189734935\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.1400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5923364782216495\n", + " w: 0.805690695346529, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 199295997\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.1600000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5958156613908069\n", + " w: 0.8031212222581566, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 209303140\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.1800000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5990966850218961\n", + " w: 0.8006766900539662, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 219341278\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.3\n", + " y: 0.06000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5137015609692951\n", + " w: -0.8579689424785198, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 229027032\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.2000000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.602195513144453\n", + " w: 0.7983486481160277, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 238579750\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.12\n", + " y: -0.7\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.19611613513818393\n", + " w: 0.9805806756909202, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 248189449\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.12\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2132313029385177\n", + " w: 0.9770017458772231, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 258103370\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.12\n", + " y: -0.74\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2297529205473612\n", + " w: 0.9732489894677302, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 268151521\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7983486481160277\n", + " w: 0.602195513144453, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 278077602\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.3\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.4672596576628944\n", + " w: -0.8841201345522873, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 288434267\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7947066772690754\n", + " w: 0.6069936549124263, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 298222303\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7913349155531438\n", + " w: 0.6113829008293401, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 308148384\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7882054380161092\n", + " w: 0.6154122094026356, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 329452991\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7825789118992277\n", + " w: 0.6225514008101024, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 339732646\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7800418122827314\n", + " w: 0.6257273935913882, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 349679470\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.72\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.77766608796156\n", + " w: 0.6286775450376476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 372056722\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.22\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6051264893449573\n", + " w: 0.7961293436955124, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 382091522\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.24\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6079025311911546\n", + " w: 0.7940116577049655, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 412238359\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.3\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.47630463962314995\n", + " w: 0.8792803251941108, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 422187566\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.3\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4847685323929452\n", + " w: 0.8746424812468178, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 431981563\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.28\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6130353373714668\n", + " w: 0.790055488642318, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 441920280\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.42\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6275230496439778\n", + " w: 0.7785979849482799, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 451804637\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.44\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6292425428779992\n", + " w: 0.7772089952081289, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 475440740\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.48\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6324713384826219\n", + " w: 0.7745837630611687, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 485437870\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.3\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5257311121191336\n", + " w: 0.85065080835204, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 495399475\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.5\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6339889056055381\n", + " w: 0.7733421413379024, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 505185365\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7438189039121905\n", + " w: 0.6683812072334675, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 515660047\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7451280178851304\n", + " w: 0.6669214623646302, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 525471210\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.39999999999999997\n", + " y: -1.52\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6354469054448959\n", + " w: 0.7721445657132514, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 536112785\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.819067476795015\n", + " w: -0.5736971923032633, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 570803880\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7533635260394922\n", + " w: 0.6576042865077321, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 580623388\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7554539549957063\n", + " w: 0.655201741360129, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 590690374\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8150216638044883\n", + " w: -0.5794304855022419, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 600275516\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8112421851755609\n", + " w: -0.5847102846637648, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 620325326\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5637378164623552\n", + " w: -0.8259537967043047, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 639937162\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.42\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5750132745107973\n", + " w: -0.8181440790816558, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 649961471\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8043897838851272\n", + " w: -0.5941019067308557, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 669656515\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.06000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7983486481160278\n", + " w: -0.6021955131444529, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 679559946\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.07999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7955906604925091\n", + " w: -0.6058345491444782, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 689200401\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.09999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7929888557099438\n", + " w: -0.6092361403592484, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 699025630\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7071067811865475\n", + " w: 0.7071067811865476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 718453407\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.12\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7905308403287534\n", + " w: -0.6124222321969663, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 728411674\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.14\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7882054380161092\n", + " w: -0.6154122094026356, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 747882127\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7071067811865475\n", + " w: 0.7071067811865476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 767148971\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7071067811865475\n", + " w: 0.7071067811865476, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 777645587\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.16000000000000003\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7860025526744504\n", + " w: -0.618223250282006, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 788507223\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.18000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.783913048024316\n", + " w: -0.6208706251202633, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 798871278\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.20000000000000007\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7819286417257465\n", + " w: -0.623367948525531, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 823041200\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.24\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7782457171509329\n", + " w: -0.6279598743042666, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 833983421\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.26\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7765341206443676\n", + " w: -0.6300752014443028, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 853881120\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7664963583466964\n", + " w: -0.6422486532809958, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 863618850\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.42000000000000004\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7652911781639873\n", + " w: -0.6436842491659838, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 873755455\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6552017413601289\n", + " w: 0.7554539549957063, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 883673667\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6532424277825721\n", + " w: 0.7571488166435519, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 893738508\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.44000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.764133282892573\n", + " w: -0.6450583895864149, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 903726816\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.45999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7630199824727258\n", + " w: -0.6463748961301956, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 914144754\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6463748961301958\n", + " w: 0.7630199824727257, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 924309968\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6436842491659837\n", + " w: 0.7652911781639874, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 934568881\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6407474392457675\n", + " w: 0.767751730118527, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 944923162\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6375295648477821\n", + " w: 0.7704258912737796, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 954938173\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.76\n", + " y: 0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7619487840078729\n", + " w: -0.647637283167765, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 964920282\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.4\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4998611817921905\n", + " w: 0.8661055356810247, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 974496364\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.38\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4956161255817646\n", + " w: 0.8685416835496848, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454728\n", + " nsecs: 994848012\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.3399999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4866443184625216\n", + " w: 0.8736001987798239, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 5209684\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.32\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.48190142905041683\n", + " w: 0.8762254348506247, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 15120983\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: 0.09999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8112421851755609\n", + " w: -0.5847102846637648, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 34841537\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.28\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4718579255320242\n", + " w: 0.8816745987679439, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 54306030\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4153730020007066\n", + " w: 0.9096511799634632, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 63890218\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.08\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4077100588947825\n", + " w: 0.9131114432948549, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 73581933\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7533635260394923\n", + " w: -0.657604286507732, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 83770036\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8320502943378438\n", + " w: -0.5547001962252289, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 114562034\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6599999999999999\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7465333575886381\n", + " w: -0.6653479886551357, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 128451824\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.39971796346469235\n", + " w: 0.9166381781726305, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 140075445\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.39138108854039505\n", + " w: 0.9202286908877246, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 149988651\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3826834323650898\n", + " w: 0.9238795325112867, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 160300254\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3736087091451055\n", + " w: 0.9275864016095363, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 170534372\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3641404642253537\n", + " w: 0.9313440407893014, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 180233716\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.3542622175095866\n", + " w: 0.9351461282843395, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 190173625\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.34395763883862607\n", + " w: 0.9389851663815341, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 199840784\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.33321075897716623\n", + " w: 0.9428523691977768, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 209982872\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.9\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.32200621957921877\n", + " w: 0.9467375531541463, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 220086812\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7205668331602575\n", + " w: -0.6933854908702645, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 230565071\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7210991742988173\n", + " w: -0.6928318560989846, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 262809038\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.15423335048016681\n", + " w: -0.9880344496016634, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 274945259\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.13608082209815303\n", + " w: -0.9906977388977382, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 284489154\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.11750042131729284\n", + " w: -0.993072832671531, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 294175386\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.27999999999999997\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.44382247843994094\n", + " w: 0.8961147290561785, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 303731441\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.27999999999999997\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.453777645066917\n", + " w: 0.8911149470396753, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 313288927\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.27999999999999997\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.46310663556107695\n", + " w: 0.8863025691598214, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 322869777\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.09853761796664212\n", + " w: -0.9951333266680702, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 332524299\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7071067811865476\n", + " w: -0.7071067811865475, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 342247724\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7229825934691132\n", + " w: -0.6908662457673518, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 351795196\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8455570886676865\n", + " w: 0.5338850155265888, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 361424207\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7071067811865476\n", + " w: -0.7071067811865475, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 371025085\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7071067811865476\n", + " w: -0.7071067811865475, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 380615949\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7709887786635176\n", + " w: -0.6368487286435748, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 399971723\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7758718496349772\n", + " w: -0.6308905395898715, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 410209894\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8407743289079371\n", + " w: 0.5413857477339129, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 420065879\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7815436979430416\n", + " w: -0.6238505014869474, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 429817914\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.8400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2855086566344833\n", + " w: 0.9583761302259008, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 439763307\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2723432423390162\n", + " w: 0.9622001654293517, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 449737310\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8280672304692729\n", + " w: 0.5606288093051838, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 459516048\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8243115549684079\n", + " w: 0.5661364326251804, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 469855308\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.8\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2586642746412806\n", + " w: 0.9659672836200511, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 480250120\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.78\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.24446768710590863\n", + " w: 0.9696574394914359, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 489985704\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.76\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2297529205473612\n", + " w: 0.9732489894677302, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 514060497\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.198787259314375\n", + " w: 0.9800426651601856, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 524521827\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.8\n", + " y: -1.1400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.805690695346529\n", + " w: 0.5923364782216496, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 534343481\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.7\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.18255737974256264\n", + " w: 0.9831952009146149, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 553424119\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.14869598393710892\n", + " w: 0.9888829578676007, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 563100337\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.13111872764660712\n", + " w: 0.9913666724579432, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 572806596\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.020000000000000018\n", + " y: -0.8200000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.24942164698879468\n", + " w: 0.968394982439189, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 592219591\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6653479886551358\n", + " w: -0.746533357588638, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 602131605\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6636470369788332\n", + " w: -0.7480458611002506, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 612154722\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.6\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.09485133899780393\n", + " w: 0.9954914482256106, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 621884822\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.58\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.07625058147116914\n", + " w: 0.997088686539622, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 631433486\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6576042865077321\n", + " w: -0.7533635260394922, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 640997409\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.07924445748219482\n", + " w: -0.9968552131369695, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 689654350\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.01922011145500681\n", + " w: -0.9998152765964606, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 699763059\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.46\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.03837651950358736\n", + " w: -0.9992633500488202, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 709666728\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.44\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.057406724906011355\n", + " w: -0.9983508741597643, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 719478130\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.42\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.07625058147116927\n", + " w: -0.997088686539622, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 729162454\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.09485133899780428\n", + " w: -0.9954914482256106, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 739078283\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.11315653826752582\n", + " w: -0.9935771725675414, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 748747825\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.13111872764660726\n", + " w: -0.9913666724579432, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 758508443\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.21999999999999997\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4056394187149922\n", + " w: 0.9140331842906817, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 768118619\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: -0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.14869598393710906\n", + " w: -0.9888829578676007, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 778162002\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.46\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.769509108149535\n", + " w: 0.6386358371363977, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 788316965\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.44\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7707373971684058\n", + " w: 0.6371529365906361, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 798148393\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.42\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7720141211097296\n", + " w: 0.6356053782081865, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 808482408\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7847357922594201\n", + " w: 0.6198304093435398, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 818536281\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7815436979430415\n", + " w: 0.6238505014869475, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 828765153\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7785979849482798\n", + " w: 0.6275230496439778, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 848044395\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: 0.30000000000000004\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.476975706616483\n", + " w: -0.8789164779987384, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 858117818\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7709887786635173\n", + " w: 0.636848728643575, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 867625951\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7687942703292954\n", + " w: 0.6394961844365034, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 877413272\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.7\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7667433203111904\n", + " w: 0.6419537995511603, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 887440681\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: 0.32000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.48190142905041666\n", + " w: -0.8762254348506247, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 897084951\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.28\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7825789118992277\n", + " w: 0.6225514008101024, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 907503843\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.26\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7843680207272645\n", + " w: 0.6202957424167875, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 917495489\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.24\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.78624155930207\n", + " w: 0.6179192588245245, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 927219152\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5524309953122208\n", + " w: 0.8335586334615874, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 937005519\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5464711272034338\n", + " w: 0.8374779442665988, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 946638345\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5400672594718117\n", + " w: 0.8416218600099494, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 975183248\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5257311121191336\n", + " w: 0.85065080835204, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 984693288\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5176837863645347\n", + " w: 0.8555720293086252, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454729\n", + " nsecs: 994870901\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6488487878132037\n", + " w: -0.7609173743274208, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 4707336\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.52\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6511308684688737\n", + " w: -0.7589654749242356, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 34533977\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6225514008101024\n", + " w: 0.7825789118992277, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 44676065\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.2000000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7902661070204828\n", + " w: 0.6127638044913315, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 54926156\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6552017413601289\n", + " w: 0.7554539549957063, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 64422369\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.657604286507732\n", + " w: 0.7533635260394923, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 74395179\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6597957320005307\n", + " w: 0.7514450026674501, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 83957433\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6618025632357402\n", + " w: 0.7496781758158658, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 93839406\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.11750042131729249\n", + " w: 0.9930728326715311, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 103564739\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.64\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1360808220981529\n", + " w: 0.9906977388977382, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 113248825\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6669214623646302\n", + " w: 0.7451280178851305, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 123576402\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.54\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6683812072334675\n", + " w: 0.7438189039121905, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 133637905\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.66\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.15423335048016673\n", + " w: 0.9880344496016635, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 143495321\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.6799999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.1719194406273956\n", + " w: 0.9851110119851283, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 153331756\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.7\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.18910752115495127\n", + " w: 0.9819563867314218, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 163374900\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.72\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.205772893716877\n", + " w: 0.9785997732532861, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 174808502\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.09999999999999998\n", + " y: -0.74\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.22189743411284238\n", + " w: 0.9750700122217567, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 185347795\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6154122094026356\n", + " w: 0.7882054380161092, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 196193218\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.61138290082934\n", + " w: 0.791334915553144, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 206784486\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6069936549124263\n", + " w: 0.7947066772690754, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 217048168\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7237282464487099\n", + " w: 0.6900850855454531, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 227622985\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7229825934691132\n", + " w: 0.6908662457673519, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 237950801\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7223009152272711\n", + " w: 0.6915789093529722, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 248067378\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7216753233664641\n", + " w: 0.6922317008371615, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 258363723\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.1800000000000002\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7924306153589356\n", + " w: 0.6099620642645399, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 268745660\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.1600000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7947066772690754\n", + " w: 0.6069936549124263, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 278952360\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.43999999999999995\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5982941042282742\n", + " w: 0.8012765844860855, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 289012908\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.25999999999999995\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.4305820312855818\n", + " w: 0.9025514469181146, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 299065113\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.25999999999999995\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.44076779540038685\n", + " w: 0.8976211620376843, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 339966058\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.43999999999999995\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5794304855022417\n", + " w: 0.8150216638044885, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 351265430\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.43999999999999995\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.5736971923032635\n", + " w: 0.8190674767950149, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 383670091\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.62\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7196150118335541\n", + " w: 0.6943732675901296, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 401098966\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.1400000000000001\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7971027464882692\n", + " w: 0.6038436979391754, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 414599180\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.12\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7996280994512247\n", + " w: 0.6004955475005809, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 425669193\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.1\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8022929282893952\n", + " w: 0.5969305296404492, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 436941385\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.08\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8051084445192626\n", + " w: 0.5931276359804638, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 447655439\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8080869942070349\n", + " w: 0.5890631628216448, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 470032453\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.02\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8145890264063119\n", + " w: 0.5800385487693183, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 480431079\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -1.0\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.818144079081656\n", + " w: 0.575013274510797, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 490664720\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8219256175556252\n", + " w: 0.5695948377626013, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 501055717\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6225514008101027\n", + " w: -0.7825789118992276, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 511538267\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6191231894884393\n", + " w: -0.7852938788999072, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 521835565\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6154122094026357\n", + " w: -0.7882054380161091, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 531538486\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -0.96\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8259537967043049\n", + " w: 0.563737816462355, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 540988683\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6069936549124266\n", + " w: -0.7947066772690753, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 559922933\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.8348410922382677\n", + " w: 0.5504910087462066, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 569701671\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5257311121191337\n", + " w: -0.8506508083520399, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 579053401\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.0\n", + " y: -0.78\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.2165835404696473\n", + " w: 0.9762640882454054, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 588583230\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7589654749242356\n", + " w: -0.6511308684688735, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 598399162\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.7609173743274209\n", + " w: -0.6488487878132035, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 608730077\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.03999999999999998\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5331718778667884\n", + " w: -0.8460069436192604, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 618813991\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5400672594718116\n", + " w: -0.8416218600099494, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 628623723\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: -0.06\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.767751730118527\n", + " w: -0.6407474392457674, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 638496398\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: 0.33999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.4866443184625217\n", + " w: -0.8736001987798239, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 648101329\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.07999999999999996\n", + " y: 0.36\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.49121312130825956\n", + " w: -0.8710394190015727, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 657718896\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.48\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6248846524332634\n", + " w: -0.780717087781073, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 667537689\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.45999999999999996\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6231467899582747\n", + " w: -0.7821049022763493, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 677129745\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.44000000000000006\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6213354700248949\n", + " w: -0.7835446596646187, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 686960220\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.42000000000000004\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6194460375118611\n", + " w: -0.7850392388988298, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 696655988\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.4\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6174734445800121\n", + " w: -0.7865917271612352, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 706870079\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.38\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.6154122094026357\n", + " w: -0.7882054380161091, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 716971874\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.48\n", + " y: -0.9199999999999999\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.6021955131444529\n", + " w: 0.7983486481160278, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 746616125\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.24\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5979254685648697\n", + " w: -0.8015517039102849, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 756603956\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.21999999999999997\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5948871592163426\n", + " w: -0.8038092235098512, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 766701936\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.20000000000000007\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5916812500238643\n", + " w: -0.8061720029684716, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 776644468\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.18000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5882940228258898\n", + " w: -0.8086471064112771, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 786627054\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.38\n", + " y: 0.16000000000000003\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5847102846637651\n", + " w: -0.8112421851755608, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 806035041\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: -1.06\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7554539549957063\n", + " w: 0.655201741360129, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 815665483\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: -1.04\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7571488166435519\n", + " w: 0.6532424277825721, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 845581769\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: -0.98\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7630199824727256\n", + " w: 0.6463748961301958, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 865120410\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.6799999999999999\n", + " y: -0.94\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.7677517301185269\n", + " w: 0.6407474392457675, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 884716749\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.43999999999999995\n", + " y: -0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5847102846637651\n", + " w: -0.8112421851755608, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 894847631\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.43999999999999995\n", + " y: 0.0\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.5895864113496071\n", + " w: -0.8077053073689017, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 914685487\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: 0.020000000000000018\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.814589026406312\n", + " w: -0.5800385487693182, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 924642562\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: 0.040000000000000036\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8112421851755609\n", + " w: -0.5847102846637648, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 934620380\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.78\n", + " y: 0.06000000000000005\n", + " z: 0.0\n", + " orientation: \n", + " x: -0.0\n", + " y: 0.0\n", + " z: 0.8080869942070349\n", + " w: -0.5890631628216447, reachable_arms=None)\n", + "CostmapLocation.Location(pose=header: \n", + " seq: 0\n", + " stamp: \n", + " secs: 1704454730\n", + " nsecs: 944352626\n", + " frame_id: \"map\"\n", + "pose: \n", + " position: \n", + " x: 0.15999999999999998\n", + " y: -0.62\n", + " z: 0.0\n", + " orientation: \n", + " x: 0.0\n", + " y: 0.0\n", + " z: 0.13273315102540856\n", + " w: 0.9911518100769761, reachable_arms=None)\n" + ] + } + ], "source": [ "from pycram.designators.location_designator import *\n", "from pycram.designators.object_designator import *\n", @@ -840,8 +8582,13 @@ }, { "cell_type": "code", - "execution_count": 29, - "metadata": {}, + "execution_count": 26, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:51.471552922Z", + "start_time": "2024-01-05T11:38:50.957762794Z" + } + }, "outputs": [], "source": [ "from pycram.designators.action_designator import *\n", @@ -861,8 +8608,13 @@ }, { "cell_type": "code", - "execution_count": 30, - "metadata": {}, + "execution_count": 27, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:38:51.532465685Z", + "start_time": "2024-01-05T11:38:51.473570735Z" + } + }, "outputs": [], "source": [ "from pycram.designators.object_designator import *\n", @@ -871,8 +8623,13 @@ }, { "cell_type": "code", - "execution_count": 31, - "metadata": {}, + "execution_count": 28, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.490830292Z", + "start_time": "2024-01-05T11:38:51.542100060Z" + } + }, "outputs": [], "source": [ "cereal_desig = ObjectDesignatorDescription(names=[\"cereal\"])\n", @@ -916,23 +8673,40 @@ }, { "cell_type": "code", - "execution_count": 32, - "metadata": {}, + "execution_count": 29, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.494121167Z", + "start_time": "2024-01-05T11:39:03.491538560Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "no_operation()\n", - "├── perform(ParkArmsAction, )\n", - "├── perform(ParkArmsAction, )\n", - "├── perform(MoveTorsoAction, )\n", - "├── perform(NavigateAction, )\n", - "├── perform(PickUpAction, )\n", - "├── perform(ParkArmsAction, )\n", - "├── perform(NavigateAction, )\n", - "├── perform(PlaceAction, )\n", - "└── perform(ParkArmsAction, )\n" + "├── perform(Motion)\n", + "├── perform(Motion)\n", + "├── perform(ParkArmsActionPerformable)\n", + "├── perform(ParkArmsActionPerformable)\n", + "├── perform(MoveTorsoActionPerformable)\n", + "├── perform(NavigateActionPerformable)\n", + "│ └── perform(Motion)\n", + "├── perform(PickUpActionPerformable)\n", + "│ ├── perform(Motion)\n", + "│ ├── perform(Motion)\n", + "│ ├── perform(Motion)\n", + "│ ├── perform(Motion)\n", + "│ └── perform(Motion)\n", + "├── perform(ParkArmsActionPerformable)\n", + "├── perform(NavigateActionPerformable)\n", + "│ └── perform(Motion)\n", + "├── perform(PlaceActionPerformable)\n", + "│ ├── perform(Motion)\n", + "│ ├── perform(Motion)\n", + "│ └── perform(Motion)\n", + "└── perform(ParkArmsActionPerformable)\n" ] } ], @@ -945,8 +8719,13 @@ }, { "cell_type": "code", - "execution_count": 33, - "metadata": {}, + "execution_count": 30, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.669289460Z", + "start_time": "2024-01-05T11:39:03.494369670Z" + } + }, "outputs": [], "source": [ "from anytree.dotexport import RenderTreeGraph, DotExporter\n", @@ -962,38 +8741,37 @@ }, { "cell_type": "code", - "execution_count": 34, - "metadata": {}, + "execution_count": 31, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.922190909Z", + "start_time": "2024-01-05T11:39:03.675069105Z" + } + }, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 10/10 [00:00<00:00, 143.50it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 22/22 [00:00<00:00, 116.49it/s]\n" ] }, { "data": { - "text/plain": [ - "pycram.orm.task.TaskTreeNode(1, 2023-11-08 13:55:09.816577, None, TaskStatus.RUNNING, None, None, 1, 1)" - ] + "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 38, 17, 681423), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" }, - "execution_count": 34, + "execution_count": 31, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "import sqlalchemy\n", "import sqlalchemy.orm\n", "import pycram.orm.base\n", - "import pycram.orm.task\n", - "import pycram.orm.object_designator\n", - "import pycram.orm.motion_designator\n", "import pycram.orm.action_designator\n", "\n", "# set description of what we are doing\n", - "pycram.orm.base.MetaData().description = \"Tutorial for getting familiar with the ORM.\"\n", + "pycram.orm.base.ProcessMetaData().description = \"Tutorial for getting familiar with the ORM.\"\n", "\n", "engine = sqlalchemy.create_engine(\"sqlite+pysqlite:///:memory:\", echo=False)\n", "session = sqlalchemy.orm.Session(bind=engine)\n", @@ -1006,15 +8784,20 @@ }, { "cell_type": "code", - "execution_count": 35, - "metadata": {}, + "execution_count": 32, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.966372243Z", + "start_time": "2024-01-05T11:39:03.922350989Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4)\n", - "pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9)\n" + "NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7)\n", + "NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16)\n" ] } ], @@ -1025,26 +8808,56 @@ }, { "cell_type": "code", - "execution_count": 36, - "metadata": {}, + "execution_count": 33, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-05T11:39:03.967890749Z", + "start_time": "2024-01-05T11:39:03.966131716Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "(pycram.orm.action_designator.NavigateAction(Navigate, 4, 4, 1, 4, 4), pycram.orm.base.Position(0.5799999999999998, 1.28, 0.0, 4, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16378361324016077, -0.9864962889104031, 4, None))\n", - "(pycram.orm.action_designator.NavigateAction(Navigate, 7, 7, 1, 9, 9), pycram.orm.base.Position(-1.9074999952316287, 0.779200015068054, 0.0, 9, None), pycram.orm.base.Quaternion(0.0, 0.0, 0.16439898730535735, 0.9863939238321437, 9, None))\n" + "(NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7), Position(id=7, x=0.5799999999999998, y=1.28, z=0.0, process_metadata_id=1), Quaternion(id=7, x=0.0, y=0.0, z=0.16378361324016077, w=-0.9864962889104031, process_metadata_id=1))\n", + "(NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16), Position(id=16, x=-1.9074999952316287, y=0.779200015068054, z=0.0, process_metadata_id=1), Quaternion(id=16, x=0.0, y=0.0, z=0.16439898730535735, w=0.9863939238321437, process_metadata_id=1))\n" ] } ], "source": [ - "navigations = session.query(pycram.orm.action_designator.NavigateAction, \n", + "navigations = (session.query(pycram.orm.action_designator.NavigateAction, \n", " pycram.orm.base.Position, \n", - " pycram.orm.base.Quaternion).\\\n", - " join(pycram.orm.base.Position).\\\n", - " join(pycram.orm.base.Quaternion).all()\n", + " pycram.orm.base.Quaternion).\n", + " join(pycram.orm.action_designator.NavigateAction.pose).\n", + " join(pycram.orm.base.Pose.position).\n", + " join(pycram.orm.base.Pose.orientation).all())\n", "print(*navigations, sep=\"\\n\")" ] + }, + { + "cell_type": "markdown", + "source": [ + "The world can also be closed with the 'exit' method" + ], + "metadata": { + "collapsed": false + } + }, + { + "cell_type": "code", + "execution_count": 34, + "outputs": [], + "source": [ + "world.exit()" + ], + "metadata": { + "collapsed": false, + "ExecuteTime": { + "end_time": "2024-01-05T11:39:04.199668442Z", + "start_time": "2024-01-05T11:39:03.966287501Z" + } + } } ], "metadata": { From 6f90a4708394c35a2bcd72fdc28404827d3f9b65 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 5 Jan 2024 13:16:29 +0100 Subject: [PATCH 03/12] [tests] updated orm tests --- src/pycram/orm/object_designator.py | 6 +- test/test_orm.py | 201 +++++++++++++++++----------- 2 files changed, 130 insertions(+), 77 deletions(-) diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 78a584282..3f2d64205 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -1,9 +1,11 @@ +from typing import Optional -from pycram.orm.base import Base, MapperArgsMixin, PoseMixin +from pycram.orm.base import Base, MapperArgsMixin, PoseMixin, Pose from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship from sqlalchemy import ForeignKey from ..enums import ObjectType + class ObjectMixin: """ ObjectMixin holds a foreign key column and its relationship to the referenced table. @@ -26,7 +28,7 @@ class Object(PoseMixin, Base): """ORM class of pycram.designators.object_designator.ObjectDesignator""" dtype: Mapped[str] = mapped_column(init=False) - type: Mapped[ObjectType] + type: Mapped[Optional[ObjectType]] name: Mapped[str] __mapper_args__ = { diff --git a/test/test_orm.py b/test/test_orm.py index 0f4e6cf63..34eea942a 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -13,7 +13,13 @@ import pycram.task import test_bullet_world import test_task_tree -from pycram.designators import action_designator, object_designator +from pycram.bullet_world import Object +from pycram.designators import action_designator, object_designator, motion_designator +from pycram.designators.actions.actions import ParkArmsActionPerformable, MoveTorsoActionPerformable, \ + SetGripperActionPerformable, PickUpActionPerformable, NavigateActionPerformable, TransportActionPerformable, \ + OpenActionPerformable, CloseActionPerformable, DetectActionPerformable, LookAtActionPerformable +from pycram.designators.object_designator import BelieveObject +from pycram.enums import ObjectType from pycram.pose import Pose from pycram.process_module import simulated_robot from pycram.task import with_tree @@ -30,11 +36,14 @@ def setUpClass(cls): def setUp(self): super().setUp() + pycram.orm.base.Base.metadata.create_all(self.engine) self.session = sqlalchemy.orm.Session(bind=self.engine) self.session.commit() def tearDown(self): super().tearDown() + pycram.task.reset_tree() + pycram.orm.base.ProcessMetaData.reset() pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() @@ -123,6 +132,21 @@ def test_node(self): action_results = self.session.query(pycram.orm.action_designator.Action).all() self.assertEqual(4, len(action_results)) + def test_metadata_existence(self): + pycram.orm.base.ProcessMetaData().description = "metadata_existence_test" + self.plan() + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.base.Pose).all() + self.assertTrue(all([r.process_metadata is not None for r in result])) + + def test_task_tree_node_parents(self): + self.plan() + pycram.orm.base.ProcessMetaData().description = "task_tree_node_parents_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.task.TaskTreeNode).all() + self.assertTrue([result[i].parent == result[result[i].parent_id - 1] for i in range(len(result)) + if result[i].parent_id is not None]) + def test_meta_data(self): self.plan() pycram.orm.base.ProcessMetaData().description = "Unittest" @@ -140,12 +164,38 @@ def test_meta_data(self): self.assertTrue(all([o.process_metadata_id for o in object_results])) def test_meta_data_alternation(self): - meta_data = pycram.orm.base.ProcessMetaData() - meta_data.description = "Test" self.plan() + pycram.orm.base.ProcessMetaData().description = "meta_data_alternation_test" pycram.task.task_tree.root.insert(self.session, ) metadata_result = self.session.query(pycram.orm.base.ProcessMetaData).first() - self.assertEqual(metadata_result.description, "Test") + self.assertEqual(metadata_result.description, "meta_data_alternation_test") + + +class MixinTestCase(ORMTestSchema, test_bullet_world.BulletWorldTest): + @with_tree + def plan(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) + self.assertEqual(description.ground().object_designator.name, "milk") + with simulated_robot: + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() + description.resolve().perform() + + def test_pose(self): + self.plan() + pycram.orm.base.ProcessMetaData().description = "pose_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.base.Pose).all() + self.assertTrue(all([r.position is not None and r.orientation is not None for r in result])) + + def test_pose_mixin(self): + self.plan() + pycram.orm.base.ProcessMetaData().description = "pose_mixin_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.base.RobotState).all() + self.assertTrue(all([r.pose is not None and r.pose_id == r.pose.id for r in result])) class ORMObjectDesignatorTestCase(test_bullet_world.BulletWorldTest): @@ -160,9 +210,9 @@ def plan(self): description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() @classmethod @@ -197,21 +247,11 @@ def test_plan_serialization(self): self.assertEqual(len(tt) - 2, len(action_results) + len(motion_results)) -class RelationshipTestCase(test_task_tree.TaskTreeTestCase): +class ORMActionDesignatorTestCase(test_bullet_world.BulletWorldTest): + engine: sqlalchemy.engine.Engine session: sqlalchemy.orm.Session - @with_tree - def plan(self): - object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) - self.assertEqual(description.ground().object_designator.name, "milk") - with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() - description.resolve().perform() - @classmethod def setUpClass(cls): super().setUpClass() @@ -236,74 +276,85 @@ def TearDownClass(cls): cls.session.commit() cls.session.close() - def test_metadata(self): - pycram.orm.base.ProcessMetaData().description = "MetaDataRelationshipTest" - self.plan() + def test_code_designator_type(self): + action = NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])) + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "code_designator_type_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.base.Position).all() - self.assertTrue(all([r.process_metadata is not None for r in result])) + result = self.session.query(pycram.orm.task.Code).filter(pycram.orm.task.Code.function == "perform").all() + self.assertEqual(result[0].designator.dtype, action_designator.NavigateAction.__name__) + self.assertEqual(result[1].designator.dtype, motion_designator.MoveMotion.__name__) - def test_task_tree_node_parents(self): - self.plan() - pycram.orm.base.ProcessMetaData().description = "taskTest" + def test_parkArmsAction(self): + action = ParkArmsActionPerformable(pycram.enums.Arms.BOTH) + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "parkArmsAction_test" pycram.task.task_tree.root.insert(self.session) - r = self.session.query(pycram.orm.task.TaskTreeNode).all() - self.assertTrue([r[i].parent == r[r[i].parent_id - 1] for i in range(len(r)) if r[i].parent_id is not None]) + result = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + self.assertTrue(all([result[i+1].dtype is not pycram.orm.action_designator.Action.dtype + if result[i].dtype is pycram.orm.action_designator.ParkArmsAction.dtype else None + for i in range(len(result)-1)])) - -class MotionDesigTest(test_bullet_world.BulletWorldTest): - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - - @with_tree - def plan(self): + def test_transportAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - description = action_designator.PlaceAction(object_description, [Pose([1.3, 0.9, 0.9], [0, 0, 0, 1])], ["left"]) - self.assertEqual(description.ground().object_designator.name, "milk") + action = TransportActionPerformable(object_description.resolve(), "left", + Pose([1.3, 0.9, 0.9], [0, 0, 0, 1])) with simulated_robot: - action_designator.NavigateAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.MoveTorsoAction.Action(0.3).perform() - action_designator.LookAtAction.Action(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - action_designator.PickUpAction.Action(object_description.resolve(), "left", "front").perform() - action_designator.SetGripperAction.Action("right", "open").perform() - description.resolve().perform() + action.perform() + pycram.orm.base.ProcessMetaData().description = "transportAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.action_designator.TransportAction).all() + milk_object = self.session.query(pycram.orm.object_designator.Object).first() + self.assertEqual(milk_object.pose, result[0].object.pose) - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) + def test_lookAt_and_detectAction(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + action = DetectActionPerformable(object_description.resolve()) + with simulated_robot: + ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + NavigateActionPerformable(Pose([0, 1, 0], [0, 0, 0, 1])).perform() + LookAtActionPerformable(object_description.resolve().pose).perform() + action.perform() + pycram.orm.base.ProcessMetaData().description = "detectAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.action_designator.DetectAction).all() + self.assertEqual(result[0].object.name, "milk") - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() + def test_setGripperAction(self): + action = SetGripperActionPerformable("left", "open") + with simulated_robot: + action.perform() + pycram.orm.base.ProcessMetaData().description = "setGripperAction_test" + pycram.task.task_tree.root.insert(self.session) + result = self.session.query(pycram.orm.action_designator.SetGripperAction).all() + self.assertEqual(result[0].gripper, "left") + self.assertEqual(result[0].motion, "open") - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() + def test_open_and_closeAction(self): + apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") + apartment_desig = BelieveObject(names=["apartment"]).resolve() + handle_desig = object_designator.ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig).resolve() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() + self.kitchen.set_pose(Pose([20, 20, 0], [0, 0, 0, 1])) - def testTest(self): - self.plan() - pycram.orm.base.ProcessMetaData().description = "Unittest" - tt = pycram.task.task_tree - tt.insert(self.session) + with simulated_robot: + ParkArmsActionPerformable(pycram.enums.Arms.BOTH).perform() + NavigateActionPerformable(Pose([1.81, 1.73, 0.0], + [0.0, 0.0, 0.594, 0.804])).perform() + OpenActionPerformable(handle_desig, arm="left").perform() + CloseActionPerformable(handle_desig, arm="left").perform() - def test_insert_base_motion(self): - motion = pycram.orm.motion_designator.Motion() - self.assertIsNone(motion.id) - self.session.add(motion) - self.session.commit() - self.assertIsNotNone(motion.id) + pycram.orm.base.ProcessMetaData().description = "open_and_closeAction_test" + pycram.task.task_tree.root.insert(self.session) + open_result = self.session.query(pycram.orm.action_designator.OpenAction).all() + close_result = self.session.query(pycram.orm.action_designator.CloseAction).all() + self.assertTrue(open_result is not None) + self.assertEqual(open_result[0].object.name, "handle_cab10_t") + self.assertTrue(close_result is not None) + self.assertEqual(close_result[0].object.name, "handle_cab10_t") + apartment.remove() if __name__ == '__main__': From af9e3e64374ca627e3dae857167f859c1a1786af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 5 Jan 2024 14:03:51 +0100 Subject: [PATCH 04/12] [tests] fixed object designator test --- test/test_object_designator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/test_object_designator.py b/test/test_object_designator.py index 625e107a2..3cdcdce0c 100644 --- a/test/test_object_designator.py +++ b/test/test_object_designator.py @@ -13,12 +13,12 @@ def test_object_grounding(self): self.assertEqual(obj.name, "milk") self.assertEqual(obj.type, ObjectType.MILK) - def test_data_copy(self): + def test_frozen_copy(self): description = ObjectDesignatorDescription(["milk"], [ObjectType.MILK]) obj = description.ground() - data_copy = obj.data_copy() - self.assertEqual(obj.pose, data_copy.pose) + frozen_copy = obj.frozen_copy() + self.assertEqual(obj.pose, frozen_copy.pose) if __name__ == '__main__': From b1f2a35a9e05f600be67da1419e4685fca3789c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Thu, 18 Jan 2024 18:58:58 +0100 Subject: [PATCH 05/12] [designator] removed MotionDesignatorDescription --- src/pycram/designator.py | 99 +- src/pycram/designators/actions/actions.py | 54 +- src/pycram/designators/motion_designator.py | 864 +++++++----------- src/pycram/process_module.py | 12 +- .../process_modules/boxy_process_modules.py | 25 +- .../process_modules/donbot_process_modules.py | 4 +- .../process_modules/pr2_process_modules.py | 46 +- test/local_transformer_tests.py | 2 +- 8 files changed, 386 insertions(+), 720 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 2319fe2dc..26d9e9efd 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -9,6 +9,7 @@ import rospy from .bullet_world import (Object as BulletWorldObject, BulletWorld) +from .enums import ObjectType from .helper import GeneratorList, bcolors from threading import Lock from time import time @@ -22,7 +23,6 @@ from .orm.action_designator import (Action as ORMAction) from .orm.object_designator import (Object as ORMObjectDesignator) -from .orm.motion_designator import (Motion as ORMMotionDesignator) from .orm.base import Quaternion, Position, Base, RobotState, ProcessMetaData from .task import with_tree @@ -361,103 +361,6 @@ def copy(self) -> Type[DesignatorDescription]: return self -class MotionDesignatorDescription(DesignatorDescription): - """ - Parent class of motion designator descriptions. - """ - - @dataclass - class Motion: - """ - Resolved motion designator which can be performed - """ - cmd: str - """ - Command of this motion designator, is used to match process modules to motion designator. Cmd is inherited by - every motion designator. - """ - - @with_tree - def perform(self): - """ - Passes this designator to the process module for execution. - - :return: The return value of the process module if there is any. - """ - raise NotImplementedError() - # return ProcessModule.perform(self) - - def to_sql(self) -> ORMMotionDesignator: - """ - Create an ORM object that corresponds to this description. - - :return: The created ORM object. - """ - return ORMMotionDesignator() - - def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: - """ - Add and commit this and all related objects to the session. - Auto-Incrementing primary keys and foreign keys have to be filled by this method. - - :param session: Session with a database that is used to add and commit the objects - :return: The completely instanced ORM motion. - """ - metadata = ProcessMetaData().insert(session) - - motion = self.to_sql() - motion.process_metadata_id = metadata.id - - return motion - - def ground(self) -> Motion: - """Fill all missing parameters and pass the designator to the process module. """ - raise NotImplementedError(f"{type(self)}.ground() is not implemented.") - - def __init__(self, resolver=None): - """ - Creates a new motion designator description - - :param resolver: An alternative resolver which overrides self.resolve() - """ - super().__init__(resolver) - - def get_slots(self): - """ - Returns a list of all slots of this description. Can be used for inspecting - different descriptions and debugging. - - :return: A list of all slots. - """ - return list(self.__dict__.keys()).remove('cmd') - - def _check_properties(self, desig: str, exclude: List[str] = []) -> None: - """ - Checks the properties of this description. It will be checked if any attribute is - None and if any attribute has to wrong type according to the type hints in - the description class. - It is possible to provide a list of attributes which should not be checked. - - :param desig: The current type of designator, will be used when raising an - Exception as output. - :param exclude: A list of properties which should not be checked. - """ - right_types = get_type_hints(self.Motion) - attributes = self.__dict__.copy() - del attributes["resolve"] - missing = [] - wrong_type = {} - current_type = {} - for k in attributes.keys(): - if attributes[k] == None and not attributes[k] in exclude: - missing.append(k) - elif type(attributes[k]) != right_types[k] and not attributes[k] in exclude: - wrong_type[k] = right_types[k] - current_type[k] = type(attributes[k]) - if missing != [] or wrong_type != {}: - raise ResolutionError(missing, wrong_type, current_type, desig) - - class ActionDesignatorDescription(DesignatorDescription): """ Abstract class for action designator descriptions. diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py index add4bc650..ef47227ef 100644 --- a/src/pycram/designators/actions/actions.py +++ b/src/pycram/designators/actions/actions.py @@ -4,9 +4,10 @@ from pycram.designators.motion_designator import * from pycram.enums import Arms from pycram.task import with_tree -from dataclasses import dataclass +from dataclasses import dataclass, field from ..location_designator import CostmapLocation from ..object_designator import BelieveObject +from ...bullet_world import BulletWorld from ...helper import multiply_quaternions from ...local_transformer import LocalTransformer from ...orm.base import Base @@ -53,15 +54,20 @@ class ParkArmsActionPerformable(ActionAbstract): def perform(self) -> None: # create the keyword arguments kwargs = dict() + left_poses = None + right_poses = None # add park left arm if wanted if self.arm in [Arms.LEFT, Arms.BOTH]: kwargs["left_arm_config"] = "park" + left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"]) # add park right arm if wanted if self.arm in [Arms.RIGHT, Arms.BOTH]: kwargs["right_arm_config"] = "park" - MoveArmJointsMotion(**kwargs).resolve().perform() + right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"]) + + MoveArmJointsMotion(left_poses, right_poses).perform() def to_sql(self) -> ORMParkArmsAction: return ORMParkArmsAction(self.arm.name) @@ -86,7 +92,7 @@ class MoveTorsoActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - MoveJointsMotion([robot_description.torso_joint], [self.position]).resolve().perform() + MoveJointsMotion([robot_description.torso_joint], [self.position]).perform() def to_sql(self) -> ORMMoveTorsoAction: return ORMMoveTorsoAction(self.position) @@ -112,7 +118,7 @@ class SetGripperActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).resolve().perform() + MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() def to_sql(self) -> ORMSetGripperAction: return ORMSetGripperAction(self.gripper, self.motion) @@ -177,7 +183,7 @@ class PickUpActionPerformable(ActionAbstract): The grasp that should be used. For example, 'left' or 'right' """ - object_at_execution: Optional[ObjectDesignatorDescription.Object] = dataclasses.field(init=False) + object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False) """ The object at the time this Action got created. It is used to be a static, information holding entity. It is not updated when the BulletWorld object is changed. @@ -231,20 +237,20 @@ def perform(self) -> None: # Perform the motion with the prepose and open gripper BulletWorld.current_bullet_world.add_vis_axis(prepose) - MoveTCPMotion(prepose, self.arm).resolve().perform() - MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() + MoveTCPMotion(prepose, self.arm).perform() + MoveGripperMotion(motion="open", gripper=self.arm).perform() # Perform the motion with the adjusted pose -> actual grasp and close gripper BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() + MoveTCPMotion(adjusted_oTm, self.arm).perform() adjusted_oTm.pose.position.z += 0.03 - MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() + MoveGripperMotion(motion="close", gripper=self.arm).perform() tool_frame = robot_description.get_tool_frame(self.arm) robot.attach(object, tool_frame) # Lift object BulletWorld.current_bullet_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm).resolve().perform() + MoveTCPMotion(adjusted_oTm, self.arm).perform() # Remove the vis axis from the world BulletWorld.current_bullet_world.remove_vis_axis() @@ -292,12 +298,12 @@ def perform(self) -> None: target_diff = self.target_location.to_transform("target").inverse_times( tcp_to_object.to_transform("object")).to_pose() - MoveTCPMotion(target_diff, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() + MoveTCPMotion(target_diff, self.arm).perform() + MoveGripperMotion("open", self.arm).perform() BulletWorld.robot.detach(self.object_designator.bullet_world_object) retract_pose = target_diff retract_pose.position.x -= 0.07 - MoveTCPMotion(retract_pose, self.arm).resolve().perform() + MoveTCPMotion(retract_pose, self.arm).perform() def to_sql(self) -> ORMPlaceAction: return ORMPlaceAction(self.arm) @@ -327,7 +333,7 @@ class NavigateActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - MoveMotion(self.target_location).resolve().perform() + MoveMotion(self.target_location).perform() def to_sql(self) -> ORMNavigateAction: return ORMNavigateAction() @@ -419,7 +425,7 @@ class LookAtActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - LookingMotion(target=self.target).resolve().perform() + LookingMotion(target=self.target).perform() def to_sql(self) -> ORMLookAtAction: return ORMLookAtAction() @@ -445,7 +451,7 @@ class DetectActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: - return DetectingMotion(object_type=self.object_designator.type).resolve().perform() + return DetectingMotion(object_type=self.object_designator.type).perform() def to_sql(self) -> ORMDetectAction: return ORMDetectAction() @@ -477,9 +483,9 @@ class OpenActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: GraspingActionPerformable(self.arm, self.object_designator).perform() - OpeningMotion(self.object_designator, self.arm).resolve().perform() + OpeningMotion(self.object_designator, self.arm).perform() - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).perform() def to_sql(self) -> ORMOpenAction: return ORMOpenAction(self.arm) @@ -511,9 +517,9 @@ class CloseActionPerformable(ActionAbstract): @with_tree def perform(self) -> None: GraspingActionPerformable(self.arm, self.object_designator).perform() - ClosingMotion(self.object_designator, self.arm).resolve().perform() + ClosingMotion(self.object_designator, self.arm).perform() - MoveGripperMotion("open", self.arm, allow_gripper_collision=True).resolve().perform() + MoveGripperMotion("open", self.arm, allow_gripper_collision=True).perform() def to_sql(self) -> ORMCloseAction: return ORMCloseAction(self.arm) @@ -557,11 +563,11 @@ def perform(self) -> None: pre_grasp = object_pose_in_gripper.copy() pre_grasp.pose.position.x -= 0.1 - MoveTCPMotion(pre_grasp, self.arm).resolve().perform() - MoveGripperMotion("open", self.arm).resolve().perform() + MoveTCPMotion(pre_grasp, self.arm).perform() + MoveGripperMotion("open", self.arm).perform() - MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).resolve().perform() - MoveGripperMotion("close", self.arm, allow_gripper_collision=True).resolve().perform() + MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() + MoveGripperMotion("close", self.arm, allow_gripper_collision=True).perform() def to_sql(self) -> ORMGraspingAction: return ORMGraspingAction(self.arm) diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index c6535a3a6..2ad0f6adb 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -1,691 +1,459 @@ -import dataclasses +import abc +import typing +from abc import ABC, abstractmethod +from dataclasses import dataclass from sqlalchemy.orm import Session from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject -from ..bullet_world import Object, BulletWorld -from ..designator import DesignatorError +from ..designator import ResolutionError +from ..enums import ObjectType +from ..orm.base import ProcessMetaData from ..plan_failures import PerceptionObjectNotFound from ..process_module import ProcessModuleManager -from ..robot_descriptions import robot_description -from ..designator import MotionDesignatorDescription from ..orm.motion_designator import (MoveMotion as ORMMoveMotion, AccessingMotion as ORMAccessingMotion, MoveTCPMotion as ORMMoveTCPMotion, LookingMotion as ORMLookingMotion, MoveGripperMotion as ORMMoveGripperMotion, DetectingMotion as ORMDetectingMotion, WorldStateDetectingMotion as ORMWorldStateDetectingMotion, - OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion) + OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion, + Motion as ORMMotionDesignator) -from typing import List, Dict, Callable, Optional +from typing import List, Dict, Callable, Optional, get_type_hints, Union, Any from ..pose import Pose from ..task import with_tree -class MoveMotion(MotionDesignatorDescription): - """ - Moves the robot to a designated location - """ +@dataclass +class BaseMotion(ABC): - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose + @abstractmethod + def perform(self): """ - Location to which the robot should be moved + Passes this designator to the process module for execution. + + :return: The return value of the process module if there is any. """ + pass + # return ProcessModule.perform(self) - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.navigate().execute(self) - # return ProcessModule.perform(self) + @abstractmethod + def to_sql(self) -> ORMMotionDesignator: + """ + Create an ORM object that corresponds to this description. - def to_sql(self) -> ORMMoveMotion: - return ORMMoveMotion() + :return: The created ORM object. + """ + return ORMMotionDesignator() - def insert(self, session, *args, **kwargs) -> ORMMoveMotion: - motion = super().insert(session) + @abstractmethod + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + """ + Add and commit this and all related objects to the session. + Auto-Incrementing primary keys and foreign keys have to be filled by this method. - pose = self.target.insert(session) - motion.pose_id = pose.id + :param session: Session with a database that is used to add and commit the objects + :return: The completely instanced ORM motion. + """ + metadata = ProcessMetaData().insert(session) - session.add(motion) - session.commit() + motion = self.to_sql() + motion.process_metadata_id = metadata.id - return motion + return motion - def __init__(self, target: Pose, resolver: Callable = None): - """ - Navigates to robot to the given target + def __post_init__(self): + right_types = get_type_hints(self) + attributes = self.__dict__.copy() - :param target: Position and Orientation of the navigation goal - :param resolver: A method with which to resolve the description - """ - super().__init__(resolver) - self.cmd: str = "navigate" - self.target: Pose = target + missing = [] + wrong_type = {} + current_type = {} - def ground(self) -> Motion: - """ - Default resolver for moving the robot, this resolver simply creates the motion designator from the input of the - designator description. + for k in attributes.keys(): + attribute = attributes[k] + attribute_type = type(attributes[k]) + right_type = right_types[k] + types = typing.get_args(right_type) + if attribute is None: + if not any([x is type(None) for x in typing.get_args(right_type)]): + missing.append(k) + elif attribute_type is not right_type: + if attribute_type not in types: + if attribute_type not in [typing.get_origin(x) for x in types if x is not type(None)]: + wrong_type[k] = right_types[k] + current_type[k] = attribute_type + if missing != [] or wrong_type != {}: + raise ResolutionError(missing, wrong_type, current_type, self.__class__) - :return: A resolved and performable motion designator - """ - return self.Motion(self.cmd, self.target) +@dataclass +class MoveMotion(BaseMotion): + """ + Moves the robot to a designated location + """ -class PickUpMotion(MotionDesignatorDescription): + target: Pose """ - Lets the robot pick up a specific object + Location to which the robot should be moved """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_desig: ObjectDesignatorDescription.Object - """ - Object designator describing the object to be picked up - """ - arm: str - """ - Arm that should be used for picking up the object - """ - grasp: str - """ - From which direction the object should be grasped, e.g. 'left', 'front', etc. - """ + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.navigate().execute(self) + # return ProcessModule.perform(self) - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.pick_up().execute(self) + def to_sql(self) -> ORMMoveMotion: + return ORMMoveMotion() - def __init__(self, object_desig: ObjectDesignatorDescription.Object, grasp: str = None, arm: str = None, - resolver: Callable = None): - """ - Motion designator implementation of the robot picking up an object. The robot will move its arm to the position - of the object and attach the object to the gripper of the robot. + def insert(self, session, *args, **kwargs) -> ORMMoveMotion: + motion = super().insert(session) - :param object_desig: Object designator of the object to be picked up - :param grasp: From which direction the object should be picked up - :param arm: Which arm should be used for picking up - :param resolver: Alternative resolver that produces a resolved and performable motion deisgnator - """ - super().__init__(resolver) - self.cmd: str = 'pick-up' - self.object_desig: ObjectDesignatorDescription.Object = object_desig - self.arm: Optional[str] = arm - self.grasp: Optional[str] = grasp + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self): - """ - Default resolver for picking up. Checks if all parameter are present and will fill missing parameter. Optional - parameter ``arm`` and ``grasp`` will default to ``'left'``. + session.add(motion) + session.commit() - :return: A resolved motion designator that can be performed - """ - arm = "left" if not self.arm else self.arm - grasp = "left" if not self.grasp else self.grasp - return self.Motion(self.cmd, self.object_desig, arm, grasp) + return motion -class PlaceMotion(MotionDesignatorDescription): +@dataclass +class PickUpMotion(BaseMotion): """ - Lets the robot place an object that was picked up + Lets the robot pick up a specific object """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object: ObjectDesignatorDescription.Object - """ - Object designator of the object to be placed - """ - target: Pose - """ - Pose at which the object should be placed - """ - arm: str - """ - Arm that is currently holding the object - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.place().execute(self) + object_desig: ObjectDesignatorDescription.Object + """ + Object designator describing the object to be picked up + """ + arm: str + """ + Arm that should be used for picking up the object + """ + grasp: str + """ + From which direction the object should be grasped, e.g. 'left', 'front', etc. + """ - def __init__(self, object_desig: ObjectDesignatorDescription.Object, target: Pose, - arm: Optional[str] = None, resolver: Optional[Callable] = None): - """ - Places the object in object_desig at the position in target. If an arm is given then the arm is used, otherwise - arm defaults to ``'left'`` + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.pick_up().execute(self) - :param object_desig: Object designator describing the object to be placed - :param target: The target pose on which to place the object - :param arm: An arm to use for placing - :param resolver: An alternative resolver that resolves the list of parameters to a resolved motion designator. - """ - super().__init__(resolver) - self.cmd: str = 'place' - self.object_desig: ObjectDesignatorDescription.Object = object_desig - self.target: Pose = target - self.arm: str = arm + def to_sql(self) -> ORMMotionDesignator: + pass - def ground(self) -> Motion: - """ - Default resolver for placing an object which returns a resolved motion designator for the input. If no arm is - given then the arm parameter will default to ``'left'``. + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass - :return: A resolved performable motion designator - """ - arm = "left" if not self.arm else self.arm - return self.Motion(self.cmd, self.object_desig, self.target, arm) +@dataclass +class PlaceMotion(BaseMotion): + """ + Lets the robot place an object that was picked up + """ -class MoveTCPMotion(MotionDesignatorDescription): + object: ObjectDesignatorDescription.Object """ - Moves the Tool center point (TCP) of the robot + Object designator of the object to be placed + """ + target: Pose + """ + Pose at which the object should be placed + """ + arm: str + """ + Arm that is currently holding the object """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose - """ - Target pose to which the TCP should be moved - """ - arm: str - """ - Arm with the TCP that should be moved to the target - """ - allow_gripper_collision: bool - """ - If the gripper can collide with something - """ + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.place().execute(self) - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_tcp().execute(self) + def to_sql(self) -> ORMMotionDesignator: + pass - def to_sql(self) -> ORMMoveTCPMotion: - return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass - def insert(self, session: Session, *args, **kwargs) -> ORMMoveTCPMotion: - motion = super().insert(session) - pose = self.target.insert(session) - motion.pose_id = pose.id +@dataclass +class MoveTCPMotion(BaseMotion): + """ + Moves the Tool center point (TCP) of the robot + """ + + target: Pose + """ + Target pose to which the TCP should be moved + """ + arm: str + """ + Arm with the TCP that should be moved to the target + """ + allow_gripper_collision: Optional[bool] = None + """ + If the gripper can collide with something + """ - session.add(motion) - session.commit() + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_tcp().execute(self) - return motion + def to_sql(self) -> ORMMoveTCPMotion: + return ORMMoveTCPMotion(self.arm, self.allow_gripper_collision) - def __init__(self, target: Pose, arm: Optional[str] = None, - resolver: Optional[Callable] = None, allow_gripper_collision: Optional[bool] = None): - """ - Moves the TCP of the given arm to the given target pose. + def insert(self, session: Session, *args, **kwargs) -> ORMMoveTCPMotion: + motion = super().insert(session) - :param target: Target pose for the TCP - :param arm: Arm that should be moved - :param resolver: Alternative resolver which returns a resolved motion designator - :param allow_gripper_collision: If the gripper should be allowed to collide with something, only used on the real robot - """ - super().__init__(resolver) - self.cmd: str = 'move-tcp' - self.target: Pose = target - self.arm: Optional[str] = arm - self.allow_gripper_collision = allow_gripper_collision + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self) -> Motion: - """ - Default resolver that returns a resolved motion designator, arm defaults to ``'left'`` if no arm is given. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - arm = "left" if not self.arm else self.arm - return self.Motion(self.cmd, self.target, arm, self.allow_gripper_collision) + return motion -class LookingMotion(MotionDesignatorDescription): +@dataclass +class LookingMotion(BaseMotion): """ Lets the robot look at a point """ + target: Pose - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - target: Pose - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.looking().execute(self) - - def to_sql(self) -> ORMLookingMotion: - return ORMLookingMotion() - - def insert(self, session: Session, *args, **kwargs) -> ORMLookingMotion: - motion = super().insert(session) - - pose = self.target.insert(session) - motion.pose_id = pose.id - - session.add(motion) - session.commit() + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.looking().execute(self) - return motion + def to_sql(self) -> ORMLookingMotion: + return ORMLookingMotion() - def __init__(self, target: Optional[Pose] = None, object: Optional[ObjectDesignatorDescription.Object] = None, - resolver: Optional[Callable] = None): - """ - Moves the head of the robot such that the camera points towards the given location. If ``target`` and ``object`` - are given ``target`` will be preferred. + def insert(self, session: Session, *args, **kwargs) -> ORMLookingMotion: + motion = super().insert(session) - :param target: Position and orientation of the target - :param object: An Object in the BulletWorld - :param resolver: Alternative resolver that returns a resolved motion designator for parameter - """ - super().__init__(resolver) - self.cmd: str = 'looking' - self.target: Optional[Pose] = target - self.object: Object = object.bullet_world_object if object else object + pose = self.target.insert(session) + motion.pose_id = pose.id - def ground(self) -> Motion: - """ - Default resolver for looking, chooses which pose to take if ``target`` and ``object`` are given. If both are given - ``target`` will be preferred. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - if not self.target and self.object: - self.target = self.object.get_pose() - return self.Motion(self.cmd, self.target) + return motion -class MoveGripperMotion(MotionDesignatorDescription): +@dataclass +class MoveGripperMotion(BaseMotion): """ Opens or closes the gripper """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - motion: str - """ - Motion that should be performed, either 'open' or 'close' - """ - gripper: str - """ - Name of the gripper that should be moved - """ - allow_gripper_collision: bool - """ - If the gripper is allowed to collide with something - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_gripper().execute(self) - - def to_sql(self) -> ORMMoveGripperMotion: - return ORMMoveGripperMotion(self.motion, self.gripper, self.allow_gripper_collision) - - def insert(self, session: Session, *args, **kwargs) -> ORMMoveGripperMotion: - motion = super().insert(session) + motion: str + """ + Motion that should be performed, either 'open' or 'close' + """ + gripper: str + """ + Name of the gripper that should be moved + """ + allow_gripper_collision: Optional[bool] = None + """ + If the gripper is allowed to collide with something + """ - session.add(motion) - session.commit() - return motion + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_gripper().execute(self) - def __init__(self, motion: str, gripper: str, resolver: Optional[Callable] = None, - allow_gripper_collision: Optional[bool] = None): - """ - Moves the gripper into a given position. + def to_sql(self) -> ORMMoveGripperMotion: + return ORMMoveGripperMotion(self.motion, self.gripper, self.allow_gripper_collision) - :param motion: Which motion to perform - :param gripper: Name of the gripper that should be moved - :param resolver: An alternative resolver that resolves the parameter to a motion designator - """ - super().__init__(resolver) - self.cmd: str = 'move-gripper' - self.motion: str = motion - self.gripper: str = gripper - self.allow_gripper_collision = allow_gripper_collision + def insert(self, session: Session, *args, **kwargs) -> ORMMoveGripperMotion: + motion = super().insert(session) - def ground(self) -> Motion: - """ - Default resolver for moving the gripper, simply returns a resolved motion designator + session.add(motion) + session.commit() + return motion - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.motion, self.gripper, self.allow_gripper_collision) - -class DetectingMotion(MotionDesignatorDescription): +@dataclass +class DetectingMotion(BaseMotion): """ Tries to detect an object in the FOV of the robot """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_type: str - """ - Type of the object that should be detected - """ + object_type: ObjectType + """ + Type of the object that should be detected + """ - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - bullet_world_object = pm_manager.detecting().execute(self) - if not bullet_world_object: - raise PerceptionObjectNotFound( - f"Could not find an object with the type {self.object_type} in the FOV of the robot") - if ProcessModuleManager.execution_type == "real": - return RealObject.Object(bullet_world_object.name, bullet_world_object.type, - bullet_world_object, bullet_world_object.get_pose()) - - return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.type, - bullet_world_object) - - def to_sql(self) -> ORMDetectingMotion: - return ORMDetectingMotion(self.object_type) - - def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: - motion = super().insert(session) - session.add(motion) - session.commit() - return motion - - def __init__(self, object_type: str, resolver: Optional[Callable] = None): - """ - Checks for every object in the FOV of the robot if it fits the given object type. If the types match an object - designator describing the object will be returned. + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + bullet_world_object = pm_manager.detecting().execute(self) + if not bullet_world_object: + raise PerceptionObjectNotFound( + f"Could not find an object with the type {self.object_type} in the FOV of the robot") + if ProcessModuleManager.execution_type == "real": + return RealObject.Object(bullet_world_object.name, bullet_world_object.type, + bullet_world_object, bullet_world_object.get_pose()) - :param object_type: Type of the object which should be detected - :param resolver: An alternative resolver which returns a resolved motion designator - """ - super().__init__(resolver) - self.cmd: str = 'detecting' - self.object_type: str = object_type + return ObjectDesignatorDescription.Object(bullet_world_object.name, bullet_world_object.type, + bullet_world_object) - def ground(self) -> Motion: - """ - Default resolver for detecting, simply returns a resolver motion designator without checking. + def to_sql(self) -> ORMDetectingMotion: + return ORMDetectingMotion(self.object_type) - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.object_type) + def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: + motion = super().insert(session) + session.add(motion) + session.commit() + return motion -class MoveArmJointsMotion(MotionDesignatorDescription): +@dataclass +class MoveArmJointsMotion(BaseMotion): """ Moves the joints of each arm into the given position """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - left_arm_poses: Dict[str, float] - """ - Target positions for the left arm joints - """ - right_arm_poses: Dict[str, float] - """ - Target positions for the right arm joints - """ - - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_arm_joints().execute(self) - - def __init__(self, left_arm_config: Optional[str] = None, right_arm_config: Optional[str] = None, - left_arm_poses: Optional[dict] = None, right_arm_poses: Optional[dict] = None, - resolver: Optional[Callable] = None): - """ - Moves the arm joints, target positions can be either be pre-defined configurations (like 'park') or a dictionary - with joint names as keys and joint positions as values. If a configuration and a dictionary are given the - dictionary will be preferred. - - :param left_arm_config: Target configuration for the left arm - :param right_arm_config: Target configuration for the right arm - :param left_arm_poses: Target Dict for the left arm - :param right_arm_poses: Target Dict for the right arm - :param resolver: An alternative resolver that returns a resolved motion designator for the given parameters. - """ - super().__init__(resolver) - self.cmd = 'move-arm-joints' - self.left_arm_config: str = left_arm_config - self.right_arm_config: str = right_arm_config - self.left_arm_poses: Dict[str, float] = left_arm_poses - self.right_arm_poses: Dict[str, float] = right_arm_poses - - def ground(self) -> Motion: - """ - Default resolver for moving the arms, returns a resolved motion designator containing the target positions for - the left and right arm joints. + left_arm_poses: Optional[Dict[str, float]] + """ + Target positions for the left arm joints + """ + right_arm_poses: Optional[Dict[str, float]] + """ + Target positions for the right arm joints + """ - :return: A resolved and performable motion designator - """ - left_poses = None - right_poses = None + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_arm_joints().execute(self) - if self.left_arm_poses: - left_poses = self.left_arm_poses - elif self.left_arm_config: - left_poses = robot_description.get_static_joint_chain("left", self.left_arm_config) + def to_sql(self) -> ORMMotionDesignator: + pass - if self.right_arm_poses: - right_poses = self.right_arm_poses - elif self.right_arm_config: - right_poses = robot_description.get_static_joint_chain("right", self.right_arm_config) - return self.Motion(self.cmd, left_poses, right_poses) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class WorldStateDetectingMotion(MotionDesignatorDescription): +@dataclass +class WorldStateDetectingMotion(BaseMotion): """ Detects an object based on the world state. """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_type: str - """ - Object type that should be detected - """ - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.world_state_detecting().execute(self) - - def __init__(self, object_type: str, resolver: Optional[Callable] = None): - """ - Tries to find an object using the belief state (BulletWorld), if there is an object in the belief state matching - the given object type an object designator will be returned. + object_type: str + """ + Object type that should be detected + """ - :param object_type: The object type which should be detected - :param resolver: An alternative resolver that returns a resolved motion designator for the input parameter - """ - super().__init__(resolver) - self.cmd: str = 'world-state-detecting' - self.object_type: str = object_type + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.world_state_detecting().execute(self) - def ground(self) -> Motion: - """ - Default resolver for world state detecting which simply returns a resolved motion designator for the input - parameter. + def to_sql(self) -> ORMMotionDesignator: + pass - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.object_type) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class MoveJointsMotion(MotionDesignatorDescription): +@dataclass +class MoveJointsMotion(BaseMotion): """ Moves any joint on the robot """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - names: list - """ - List of joint names that should be moved - """ - positions: list - """ - Target positions of joints, should correspond to the list of names - """ - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.move_joints().execute(self) - - def __init__(self, names: List[str], positions: List[float], resolver: Optional[Callable] = None): - """ - Moves the joints given by the list of names to the positions given by the list of positions. The index of a - joint name should correspond to the index of the target position. + names: list + """ + List of joint names that should be moved + """ + positions: list + """ + Target positions of joints, should correspond to the list of names + """ - :param names: List of joint names that should be moved - :param positions: List of joint positions that the joints should be moved in - :param resolver: An alternative resolver that resolves the input parameters to a performable motion designator. - """ - super().__init__(resolver) - self.cmd: str = "move-joints" - self.names: List[str] = names - self.positions: List[float] = positions + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.move_joints().execute(self) - def ground(self) -> Motion: - """ - Default resolver for move joints, checks if the length of both list match and checks if the target positions are - within the joint limits as stated in the URDF. + def to_sql(self) -> ORMMotionDesignator: + pass - :return: A resolved motion designator - """ - if len(self.names) != len(self.positions): - raise DesignatorError("[Motion Designator][Move Joints] The length of names and positions does not match") - for i in range(len(self.names)): - lower, upper = BulletWorld.robot.get_joint_limits(self.names[i]) - if self.positions[i] < lower or self.positions[i] > upper: - raise DesignatorError( - f"[Motion Designator][Move Joints] The given configuration for the Joint {self.names[i]} violates its limits") - return self.Motion(self.cmd, self.names, self.positions) + def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: + pass -class OpeningMotion(MotionDesignatorDescription): +@dataclass +class OpeningMotion(BaseMotion): """ Designator for opening container """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_part: ObjectPart.Object - """ - Object designator for the drawer handle - """ - arm: str - """ - Arm that should be used - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.open().execute(self) - - def to_sql(self) -> ORMOpeningMotion: - return ORMOpeningMotion(self.arm) - - def insert(self, session: Session, *args, **kwargs) -> ORMOpeningMotion: - motion = super().insert(session) - - op = self.object_part.insert(session) - motion.object_id = op.id + object_part: ObjectPart.Object + """ + Object designator for the drawer handle + """ + arm: str + """ + Arm that should be used + """ - session.add(motion) - session.commit() + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.open().execute(self) - return motion + def to_sql(self) -> ORMOpeningMotion: + return ORMOpeningMotion(self.arm) - def __init__(self, object_part: ObjectPart.Object, arm: str, resolver: Optional[Callable] = None): - """ - Lets the robot open a container specified by the given parameter. This motion designator assumes that the handle - is already grasped. + def insert(self, session: Session, *args, **kwargs) -> ORMOpeningMotion: + motion = super().insert(session) - :param object_part: Object designator describing the handle of the drawer - :param arm: Arm that should be used - :param resolver: An alternative resolver - """ - super().__init__(resolver) - self.cmd: str = 'open' - self.objet_part = object_part - self.arm: str = arm + op = self.object_part.insert(session) + motion.object_id = op.id - def ground(self) -> Motion: - """ - Default resolver for opening motion designator, returns a resolved motion designator for the input parameters. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.objet_part, self.arm) + return motion -class ClosingMotion(MotionDesignatorDescription): +@dataclass +class ClosingMotion(BaseMotion): """ Designator for closing a container """ - @dataclasses.dataclass - class Motion(MotionDesignatorDescription.Motion): - # cmd: str - object_part: ObjectPart.Object - """ - Object designator for the drawer handle - """ - arm: str - """ - Arm that should be used - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.close().execute(self) - - def to_sql(self) -> ORMClosingMotion: - return ORMClosingMotion(self.arm) - - def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: - motion = super().insert(session) - - op = self.object_part.insert(session) - motion.object_id = op.id + object_part: ObjectPart.Object + """ + Object designator for the drawer handle + """ + arm: str + """ + Arm that should be used + """ - session.add(motion) - session.commit() + @with_tree + def perform(self): + pm_manager = ProcessModuleManager.get_manager() + return pm_manager.close().execute(self) - return motion + def to_sql(self) -> ORMClosingMotion: + return ORMClosingMotion(self.arm) - def __init__(self, object_part: ObjectPart.Object, arm: str, resolver: Optional[Callable] = None): - """ - Lets the robot close a container specified by the given parameter. This assumes that the handle is already grasped + def insert(self, session: Session, *args, **kwargs) -> ORMClosingMotion: + motion = super().insert(session) - :param object_part: Object designator describing the handle of the drawer - :param arm: Arm that should be used - :param resolver: An alternative resolver - """ - super().__init__(resolver) - self.cmd: str = 'close' - self.objet_part = object_part - self.arm: str = arm + op = self.object_part.insert(session) + motion.object_id = op.id - def ground(self) -> Motion: - """ - Default resolver for opening motion designator, returns a resolved motion designator for the input parameters. + session.add(motion) + session.commit() - :return: A resolved motion designator - """ - return self.Motion(self.cmd, self.objet_part, self.arm) + return motion diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 92990c9ae..b1539a099 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -5,19 +5,13 @@ """ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations - import inspect import time from abc import ABC -from threading import Lock - import rospy - -from .designator import MotionDesignatorDescription -from .fluent import Fluent from typing import Callable, List, Type, Any, Union -from .robot_descriptions import robot_description +from .robot_descriptions import robot_description class ProcessModule: @@ -33,14 +27,14 @@ def __init__(self, lock): """Create a new process module.""" self._lock = lock - def _execute(self, designator: MotionDesignatorDescription.Motion) -> Any: + def _execute(self, designator) -> Any: """ Helper method for internal usage only. This method is to be overwritten instead of the execute method. """ pass - def execute(self, designator: MotionDesignatorDescription.Motion) -> Any: + def execute(self, designator) -> Any: """ Execute the given designator. If there is already another process module of the same kind the `self._lock` will lock this thread until the execution of that process module is finished. This implicitly queues the execution of diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index 4ce16bf96..a455e6743 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -1,19 +1,14 @@ -import time from threading import Lock - import numpy as np -import pybullet as p - import pycram.bullet_world_reasoning as btr import pycram.helper as helper -from ..bullet_world import BulletWorld +from ..bullet_world import BulletWorld, Object from ..designators.motion_designator import * from ..enums import JointType from ..external_interfaces.ik import request_ik -from ..local_transformer import LocalTransformer as local_tf, LocalTransformer +from ..local_transformer import LocalTransformer from ..process_module import ProcessModule, ProcessModuleManager from ..robot_descriptions import robot_description -from tf.transformations import euler_from_quaternion, quaternion_from_euler def _park_arms(arm): @@ -37,7 +32,7 @@ class BoxyNavigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) @@ -48,7 +43,7 @@ class BoxyPickUp(ProcessModule): The object has to be reachable for this process module to succeed. """ - def _execute(self, desig: PickUpMotion.Motion): + def _execute(self, desig: PickUpMotion): object = desig.object_desig.bullet_world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) @@ -70,7 +65,7 @@ class BoxyPlace(ProcessModule): This process module places an object at the given position in world coordinate frame. """ - def _execute(self, desig: PlaceMotion.Motion): + def _execute(self, desig: PlaceMotion): """ :param desig: A PlaceMotion @@ -96,7 +91,7 @@ class BoxyOpen(ProcessModule): Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -115,7 +110,7 @@ class BoxyClose(ProcessModule): """ Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -210,7 +205,7 @@ class BoxyMoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -223,7 +218,7 @@ class BoxyMoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -237,7 +232,7 @@ class BoxyWorldStateDetecting(ProcessModule): This process module detectes an object even if it is not in the field of view of the robot. """ - def _execute(self, desig: WorldStateDetectingMotion.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] diff --git a/src/pycram/process_modules/donbot_process_modules.py b/src/pycram/process_modules/donbot_process_modules.py index ed5c8ae80..cd73d0895 100644 --- a/src/pycram/process_modules/donbot_process_modules.py +++ b/src/pycram/process_modules/donbot_process_modules.py @@ -161,7 +161,7 @@ class DonbotMoveJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.left_arm_poses: robot.set_joint_states(desig.left_arm_poses) @@ -172,7 +172,7 @@ class DonbotWorldStateDetecting(ProcessModule): This process module detectes an object even if it is not in the field of view of the robot. """ - def _execute(self, desig: WorldStateDetectingMotion.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 3930a8a9b..6bdc8d3df 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -50,7 +50,7 @@ class Pr2Navigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) @@ -61,7 +61,7 @@ class Pr2PickUp(ProcessModule): The object has to be reachable for this process module to succeed. """ - def _execute(self, desig: PickUpMotion.Motion): + def _execute(self, desig: PickUpMotion): object = desig.object_desig.bullet_world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) @@ -83,7 +83,7 @@ class Pr2Place(ProcessModule): This process module places an object at the given position in world coordinate frame. """ - def _execute(self, desig: PlaceMotion.Motion): + def _execute(self, desig: PlaceMotion): """ :param desig: A PlaceMotion @@ -109,7 +109,7 @@ class Pr2MoveHead(ProcessModule): This point can either be a position or an object. """ - def _execute(self, desig: LookingMotion.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -133,7 +133,7 @@ class Pr2MoveGripper(ProcessModule): Furthermore, it can only moved one gripper at a time. """ - def _execute(self, desig: MoveGripperMotion.Motion): + def _execute(self, desig: MoveGripperMotion): robot = BulletWorld.robot gripper = desig.gripper motion = desig.motion @@ -147,7 +147,7 @@ class Pr2Detecting(ProcessModule): the field of view of the robot. """ - def _execute(self, desig: DetectingMotion.Motion): + def _execute(self, desig: DetectingMotion): robot = BulletWorld.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" @@ -166,7 +166,7 @@ class Pr2MoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -179,7 +179,7 @@ class Pr2MoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -192,7 +192,7 @@ class PR2MoveJoints(ProcessModule): """ Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot """ - def _execute(self, desig: MoveJointsMotion.Motion): + def _execute(self, desig: MoveJointsMotion): robot = BulletWorld.robot robot.set_joint_states(dict(zip(desig.names, desig.positions))) @@ -202,7 +202,7 @@ class Pr2WorldStateDetecting(ProcessModule): This process module detectes an object even if it is not in the field of view of the robot. """ - def _execute(self, desig: WorldStateDetectingMotion.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] @@ -212,7 +212,7 @@ class Pr2Open(ProcessModule): Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -232,7 +232,7 @@ class Pr2Close(ProcessModule): Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -266,20 +266,20 @@ class Pr2NavigationReal(ProcessModule): Process module for the real PR2 that sends a cartesian goal to giskard to move the robot base """ - def _execute(self, designator: MoveMotion.Motion) -> Any: + def _execute(self, designator: MoveMotion) -> Any: rospy.logdebug(f"Sending goal to giskard to Move the robot") giskard.achieve_cartesian_goal(designator.target, robot_description.base_link, "map") class Pr2PickUpReal(ProcessModule): - def _execute(self, designator: PickUpMotion.Motion) -> Any: + def _execute(self, designator: PickUpMotion) -> Any: pass class Pr2PlaceReal(ProcessModule): - def _execute(self, designator: MotionDesignatorDescription.Motion) -> Any: + def _execute(self, designator: BaseMotion) -> Any: pass @@ -289,7 +289,7 @@ class Pr2MoveHeadReal(ProcessModule): as the simulated one """ - def _execute(self, desig: LookingMotion.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -314,7 +314,7 @@ class Pr2DetectingReal(ProcessModule): for perception of the environment. """ - def _execute(self, designator: DetectingMotion.Motion) -> Any: + def _execute(self, designator: DetectingMotion) -> Any: query_result = query(ObjectDesignatorDescription(types=[designator.object_type])) # print(query_result) obj_pose = query_result["ClusterPoseBBAnnotator"] @@ -347,7 +347,7 @@ class Pr2MoveTCPReal(ProcessModule): Moves the tool center point of the real PR2 while avoiding all collisions """ - def _execute(self, designator: MoveTCPMotion.Motion) -> Any: + def _execute(self, designator: MoveTCPMotion) -> Any: lt = LocalTransformer() pose_in_map = lt.transform_pose(designator.target, "map") @@ -362,7 +362,7 @@ class Pr2MoveArmJointsReal(ProcessModule): Moves the arm joints of the real PR2 to the given configuration while avoiding all collisions """ - def _execute(self, designator: MoveArmJointsMotion.Motion) -> Any: + def _execute(self, designator: MoveArmJointsMotion) -> Any: joint_goals = {} if designator.left_arm_poses: joint_goals.update(designator.left_arm_poses) @@ -377,7 +377,7 @@ class Pr2MoveJointsReal(ProcessModule): Moves any joint using giskard, avoids all collisions while doint this. """ - def _execute(self, designator: MoveJointsMotion.Motion) -> Any: + def _execute(self, designator: MoveJointsMotion) -> Any: name_to_position = dict(zip(designator.names, designator.positions)) giskard.avoid_all_collisions() giskard.achieve_joint_goal(name_to_position) @@ -388,7 +388,7 @@ class Pr2MoveGripperReal(ProcessModule): Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard """ - def _execute(self, designator: MoveGripperMotion.Motion) -> Any: + def _execute(self, designator: MoveGripperMotion) -> Any: def activate_callback(): rospy.loginfo("Started gripper Movement") @@ -414,7 +414,7 @@ class Pr2OpenReal(ProcessModule): Tries to open an already grasped container """ - def _execute(self, designator: OpeningMotion.Motion) -> Any: + def _execute(self, designator: OpeningMotion) -> Any: giskard.achieve_open_container_goal(robot_description.get_tool_frame(designator.arm), designator.object_part.name) @@ -424,7 +424,7 @@ class Pr2CloseReal(ProcessModule): Tries to close an already grasped container """ - def _execute(self, designator: ClosingMotion.Motion) -> Any: + def _execute(self, designator: ClosingMotion) -> Any: giskard.achieve_close_container_goal(robot_description.get_tool_frame(designator.arm), designator.object_part.name) diff --git a/test/local_transformer_tests.py b/test/local_transformer_tests.py index 7629b2248..845ae9ac1 100644 --- a/test/local_transformer_tests.py +++ b/test/local_transformer_tests.py @@ -10,7 +10,7 @@ import bullet_world_testcase -class TestLocalTransformer(test_bullet_world.BulletWorldTestCase): +class TestLocalTransformer(bullet_world_testcase.BulletWorldTestCase): def test_singelton(self): l1 = LocalTransformer() From 7520c8a4fa4f72f2d29ad3d1c4285919a795a040 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 19 Jan 2024 14:51:57 +0100 Subject: [PATCH 06/12] merge dev into tests --- src/pycram/designators/actions/actions.py | 3 ++- .../default_process_modules.py | 26 +++++++++---------- test/test_language.py | 3 +++ 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py index ef47227ef..2497be648 100644 --- a/src/pycram/designators/actions/actions.py +++ b/src/pycram/designators/actions/actions.py @@ -301,7 +301,8 @@ def perform(self) -> None: MoveTCPMotion(target_diff, self.arm).perform() MoveGripperMotion("open", self.arm).perform() BulletWorld.robot.detach(self.object_designator.bullet_world_object) - retract_pose = target_diff + retract_pose = local_tf.transform_pose(target_diff, BulletWorld.robot.get_link_tf_frame( + robot_description.get_tool_frame(self.arm))) retract_pose.position.x -= 0.07 MoveTCPMotion(retract_pose, self.arm).perform() diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 390558bd9..9705722a4 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -5,7 +5,7 @@ from ..robot_descriptions import robot_description from ..process_module import ProcessModule, ProcessModuleManager -from ..bullet_world import BulletWorld +from ..bullet_world import BulletWorld, Object from ..external_interfaces.ik import request_ik, IKError from ..helper import _apply_ik from ..local_transformer import LocalTransformer @@ -18,7 +18,7 @@ class DefaultNavigation(ProcessModule): The process module to move the robot from one position to another. """ - def _execute(self, desig: MoveMotion.Motion): + def _execute(self, desig: MoveMotion): robot = BulletWorld.robot robot.set_pose(desig.target) @@ -29,7 +29,7 @@ class DefaultPickUp(ProcessModule): The object has to be reachable for this process module to succeed. """ - def _execute(self, desig: PickUpMotion.Motion): + def _execute(self, desig: PickUpMotion): object = desig.object_desig.bullet_world_object robot = BulletWorld.robot grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) @@ -51,7 +51,7 @@ class DefaultPlace(ProcessModule): This process module places an object at the given position in world coordinate frame. """ - def _execute(self, desig: PlaceMotion.Motion): + def _execute(self, desig: PlaceMotion): """ :param desig: A PlaceMotion @@ -71,7 +71,7 @@ class DefaultMoveHead(ProcessModule): This point can either be a position or an object. """ - def _execute(self, desig: LookingMotion.Motion): + def _execute(self, desig: LookingMotion): target = desig.target robot = BulletWorld.robot @@ -101,7 +101,7 @@ class DefaultMoveGripper(ProcessModule): Furthermore, it can only moved one gripper at a time. """ - def _execute(self, desig: MoveGripperMotion.Motion): + def _execute(self, desig: MoveGripperMotion): robot = BulletWorld.robot gripper = desig.gripper motion = desig.motion @@ -115,7 +115,7 @@ class DefaultDetecting(ProcessModule): the field of view of the robot. """ - def _execute(self, desig: DetectingMotion.Motion): + def _execute(self, desig: DetectingMotion): robot = BulletWorld.robot object_type = desig.object_type # Should be "wide_stereo_optical_frame" @@ -134,7 +134,7 @@ class DefaultMoveTCP(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def _execute(self, desig: MoveTCPMotion.Motion): + def _execute(self, desig: MoveTCPMotion): target = desig.target robot = BulletWorld.robot @@ -147,7 +147,7 @@ class DefaultMoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ - def _execute(self, desig: MoveArmJointsMotion.Motion): + def _execute(self, desig: MoveArmJointsMotion): robot = BulletWorld.robot if desig.right_arm_poses: @@ -159,7 +159,7 @@ def _execute(self, desig: MoveArmJointsMotion.Motion): class DefaultMoveJoints(ProcessModule): - def _execute(self, desig: MoveJointsMotion.Motion): + def _execute(self, desig: MoveJointsMotion): robot = BulletWorld.robot for joint, pose in zip(desig.names, desig.positions): robot.set_joint_state(joint, pose) @@ -170,7 +170,7 @@ class DefaultWorldStateDetecting(ProcessModule): This process module detectes an object even if it is not in the field of view of the robot. """ - def _execute(self, desig: WorldStateDetectingMotion.Motion): + def _execute(self, desig: WorldStateDetectingMotion): obj_type = desig.object_type return list(filter(lambda obj: obj.type == obj_type, BulletWorld.current_bullet_world.objects))[0] @@ -180,7 +180,7 @@ class DefaultOpen(ProcessModule): Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. """ - def _execute(self, desig: OpeningMotion.Motion): + def _execute(self, desig: OpeningMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) @@ -199,7 +199,7 @@ class DefaultClose(ProcessModule): """ Low-level implementation that lets the robot close a grasped container, in simulation """ - def _execute(self, desig: ClosingMotion.Motion): + def _execute(self, desig: ClosingMotion): part_of_object = desig.object_part.bullet_world_object container_joint = part_of_object.find_joint_above(desig.object_part.name, JointType.PRISMATIC) diff --git a/test/test_language.py b/test/test_language.py index 951db67c4..e63d5ec57 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -1,6 +1,8 @@ import threading +import time import unittest from pycram.designators.action_designator import * +from pycram.designators.object_designator import BelieveObject from pycram.enums import ObjectType, State from pycram.fluent import Fluent from pycram.plan_failures import PlanFailure @@ -8,6 +10,7 @@ from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Repeat, Code, RenderTree from pycram.process_module import simulated_robot import test_bullet_world +from pycram.robot_descriptions import robot_description class LanguageTestCase(test_bullet_world.BulletWorldTest): From 5833a9de6cfb831817c1377c9d54178a7a7fa4e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Mon, 22 Jan 2024 16:24:09 +0100 Subject: [PATCH 07/12] [test] updated orm querying; added docstrings to actions and motions --- src/pycram/designators/action_designator.py | 3 +- src/pycram/designators/actions/actions.py | 134 ++++++++++++------ src/pycram/designators/motion_designator.py | 21 +-- .../resolver/location/database_location.py | 26 ++-- test/test_database_merger.py | 4 +- test/test_database_resolver.py | 23 +-- test/test_orm.py | 70 ++++----- 7 files changed, 162 insertions(+), 119 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index f8b9d347f..828dc2bdb 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1,6 +1,5 @@ import itertools -from typing import Union -from .motion_designator import * +from typing_extensions import List, Union, Callable from .object_designator import ObjectDesignatorDescription, ObjectPart from ..enums import Arms from ..designator import ActionDesignatorDescription diff --git a/src/pycram/designators/actions/actions.py b/src/pycram/designators/actions/actions.py index 2497be648..83ca6c29f 100644 --- a/src/pycram/designators/actions/actions.py +++ b/src/pycram/designators/actions/actions.py @@ -1,5 +1,5 @@ import abc -from typing import Union +from typing_extensions import Union from pycram.designator import ActionDesignatorDescription from pycram.designators.motion_designator import * from pycram.enums import Arms @@ -27,62 +27,36 @@ class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): @abc.abstractmethod def perform(self) -> None: - """Perform the action.""" + """ + Perform the action. Will be overwritten by each action. + """ pass @abc.abstractmethod def to_sql(self) -> Action: - """Convert this action to its ORM equivalent.""" + """ + Convert this action to its ORM equivalent. Will be overwritten by each action. + """ pass @abc.abstractmethod def insert(self, session: Session, **kwargs) -> Action: - """Insert this action into the database.""" - action = super().insert(session) - return action - - -@dataclass -class ParkArmsActionPerformable(ActionAbstract): - - arm: Arms - """ - Entry from the enum for which arm should be parked - """ - - @with_tree - def perform(self) -> None: - # create the keyword arguments - kwargs = dict() - left_poses = None - right_poses = None - - # add park left arm if wanted - if self.arm in [Arms.LEFT, Arms.BOTH]: - kwargs["left_arm_config"] = "park" - left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"]) - - # add park right arm if wanted - if self.arm in [Arms.RIGHT, Arms.BOTH]: - kwargs["right_arm_config"] = "park" - right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"]) + """ + Insert this action into the database. - MoveArmJointsMotion(left_poses, right_poses).perform() - - def to_sql(self) -> ORMParkArmsAction: - return ORMParkArmsAction(self.arm.name) + :param session: Session with a database that is used to add and commit the objects + :param kwargs: Possible extra keyword arguments + :return: The completely instanced ORM object + """ - def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: action = super().insert(session) - session.add(action) - session.commit() return action @dataclass class MoveTorsoActionPerformable(ActionAbstract): """ - Performable Move Torso Action designator. + Move the torso of the robot up and down. """ position: float @@ -106,6 +80,9 @@ def insert(self, session: Session, **kwargs) -> ORMMoveTorsoAction: @dataclass class SetGripperActionPerformable(ActionAbstract): + """ + Set the gripper state of the robot. + """ gripper: str """ @@ -132,6 +109,11 @@ def insert(self, session: Session, *args, **kwargs) -> ORMSetGripperAction: @dataclass class ReleaseActionPerformable(ActionAbstract): + """ + Releases an Object from the robot. + + Note: This action can not ve used yet. + """ gripper: str @@ -149,6 +131,11 @@ def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: @dataclass class GripActionPerformable(ActionAbstract): + """ + Grip an object with the robot. + + Note: This action can not be used yet. + """ gripper: str object_designator: ObjectDesignatorDescription.Object @@ -165,8 +152,51 @@ def insert(self, session: Session, *args, **kwargs) -> Base: raise NotImplementedError() +@dataclass +class ParkArmsActionPerformable(ActionAbstract): + """ + Park the arms of the robot. + """ + + arm: Arms + """ + Entry from the enum for which arm should be parked + """ + + @with_tree + def perform(self) -> None: + # create the keyword arguments + kwargs = dict() + left_poses = None + right_poses = None + + # add park left arm if wanted + if self.arm in [Arms.LEFT, Arms.BOTH]: + kwargs["left_arm_config"] = "park" + left_poses = robot_description.get_static_joint_chain("left", kwargs["left_arm_config"]) + + # add park right arm if wanted + if self.arm in [Arms.RIGHT, Arms.BOTH]: + kwargs["right_arm_config"] = "park" + right_poses = robot_description.get_static_joint_chain("right", kwargs["right_arm_config"]) + + MoveArmJointsMotion(left_poses, right_poses).perform() + + def to_sql(self) -> ORMParkArmsAction: + return ORMParkArmsAction(self.arm.name) + + def insert(self, session: Session, **kwargs) -> ORMParkArmsAction: + action = super().insert(session) + session.add(action) + session.commit() + return action + + @dataclass class PickUpActionPerformable(ActionAbstract): + """ + Let the robot pick up an object. + """ object_designator: ObjectDesignatorDescription.Object """ @@ -272,6 +302,9 @@ def insert(self, session: Session, **kwargs) -> ORMPickUpAction: @dataclass class PlaceActionPerformable(ActionAbstract): + """ + Places an Object at a position using an arm. + """ object_designator: ObjectDesignatorDescription.Object """ @@ -326,6 +359,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMPlaceAction: @dataclass class NavigateActionPerformable(ActionAbstract): + """ + Navigates the Robot to a position. + """ target_location: Pose """ @@ -353,6 +389,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMNavigateAction: @dataclass class TransportActionPerformable(ActionAbstract): + """ + Transports an object to a position using an arm + """ object_designator: ObjectDesignatorDescription.Object """ @@ -418,6 +457,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMTransportAction: @dataclass class LookAtActionPerformable(ActionAbstract): + """ + Lets the robot look at a position. + """ target: Pose """ @@ -444,6 +486,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMLookAtAction: @dataclass class DetectActionPerformable(ActionAbstract): + """ + Detects an object that fits the object description and returns an object designator describing the object. + """ object_designator: ObjectDesignatorDescription.Object """ @@ -471,6 +516,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMDetectAction: @dataclass class OpenActionPerformable(ActionAbstract): + """ + Opens a container like object + """ object_designator: ObjectPart.Object """ @@ -505,6 +553,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMOpenAction: @dataclass class CloseActionPerformable(ActionAbstract): + """ + Closes a container like object. + """ object_designator: ObjectPart.Object """ @@ -539,6 +590,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMCloseAction: @dataclass class GraspingActionPerformable(ActionAbstract): + """ + Grasps an object described by the given Object Designator description + """ arm: str """ diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 2ad0f6adb..44b131aa5 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -1,5 +1,3 @@ -import abc -import typing from abc import ABC, abstractmethod from dataclasses import dataclass @@ -17,7 +15,7 @@ OpeningMotion as ORMOpeningMotion, ClosingMotion as ORMClosingMotion, Motion as ORMMotionDesignator) -from typing import List, Dict, Callable, Optional, get_type_hints, Union, Any +from typing_extensions import Dict, Optional, get_type_hints, get_args, get_origin from ..pose import Pose from ..task import with_tree @@ -28,9 +26,7 @@ class BaseMotion(ABC): @abstractmethod def perform(self): """ - Passes this designator to the process module for execution. - - :return: The return value of the process module if there is any. + Passes this designator to the process module for execution. Will be overwritten by each motion. """ pass # return ProcessModule.perform(self) @@ -38,7 +34,7 @@ def perform(self): @abstractmethod def to_sql(self) -> ORMMotionDesignator: """ - Create an ORM object that corresponds to this description. + Create an ORM object that corresponds to this description. Will be overwritten by each motion. :return: The created ORM object. """ @@ -51,6 +47,8 @@ def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: Auto-Incrementing primary keys and foreign keys have to be filled by this method. :param session: Session with a database that is used to add and commit the objects + :param args: Possible extra arguments + :param kwargs: Possible extra keyword arguments :return: The completely instanced ORM motion. """ metadata = ProcessMetaData().insert(session) @@ -61,6 +59,9 @@ def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: return motion def __post_init__(self): + """ + Checks if types are missing or wrong + """ right_types = get_type_hints(self) attributes = self.__dict__.copy() @@ -72,13 +73,13 @@ def __post_init__(self): attribute = attributes[k] attribute_type = type(attributes[k]) right_type = right_types[k] - types = typing.get_args(right_type) + types = get_args(right_type) if attribute is None: - if not any([x is type(None) for x in typing.get_args(right_type)]): + if not any([x is type(None) for x in get_args(right_type)]): missing.append(k) elif attribute_type is not right_type: if attribute_type not in types: - if attribute_type not in [typing.get_origin(x) for x in types if x is not type(None)]: + if attribute_type not in [get_origin(x) for x in types if x is not type(None)]: wrong_type[k] = right_types[k] current_type[k] = attribute_type if missing != [] or wrong_type != {}: diff --git a/src/pycram/resolver/location/database_location.py b/src/pycram/resolver/location/database_location.py index 4febdf360..d613b99a3 100644 --- a/src/pycram/resolver/location/database_location.py +++ b/src/pycram/resolver/location/database_location.py @@ -1,6 +1,7 @@ import numpy as np import sqlalchemy.orm import sqlalchemy.sql +from sqlalchemy import select, Select from tf import transformations import pycram.designators.location_designator import pycram.task @@ -34,25 +35,24 @@ def __init__(self, target, session: sqlalchemy.orm.Session = None, super().__init__(target, reachable_for, None, reachable_arm, resolver) self.session = session - def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query: + def create_query_from_occupancy_costmap(self) -> Select: """ Create a query that queries all relative robot positions from an object that are not occluded using an OccupancyCostmap. """ robot_pos = sqlalchemy.orm.aliased(Position) - object_pos = sqlalchemy.orm.aliased(Position) # query all relative robot positions in regard to an objects position # make sure to order the joins() correctly - query = (self.session.query(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x, - Position.y).join(TaskTreeNode.code) - .join(Code.designator.of_type(PickUpAction)) - .join(PickUpAction.robot_state) - .join(RobotState.pose) - .join(orm.base.Pose.position) - .join(PickUpAction.object).filter(Object.type == self.target.type) - .filter(TaskTreeNode.status == "SUCCEEDED")) + query = (select(PickUpAction.arm, PickUpAction.grasp, RobotState.torso_height, Position.x, Position.y) + .join(TaskTreeNode.code) + .join(Code.designator.of_type(PickUpAction)) + .join(PickUpAction.robot_state) + .join(RobotState.pose) + .join(orm.base.Pose.position) + .join(PickUpAction.object).where(Object.type == self.target.type) + .where(TaskTreeNode.status == "SUCCEEDED")) # create Occupancy costmap for the target object position, orientation = self.target.pose.to_list() @@ -94,7 +94,7 @@ def create_query_from_occupancy_costmap(self) -> sqlalchemy.orm.Query: robot_pos.y >= rectangle[1][0], robot_pos.y < rectangle[1][1])) # query = self.model.bind({"x": list(rectangle[0]), "y": list(rectangle[1])}) - return query.filter(sqlalchemy.or_(*filters)) + return query.where(sqlalchemy.or_(*filters)) def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLocation.Location: """ @@ -112,7 +112,7 @@ def sample_to_location(self, sample: sqlalchemy.engine.row.Row) -> JPTCostmapLoc return result def __iter__(self) -> JPTCostmapLocation.Location: - query = self.create_query_from_occupancy_costmap().limit(200) - samples = query.all() + statement = self.create_query_from_occupancy_costmap().limit(200) + samples = self.session.execute(statement).all() for sample in samples: yield self.sample_to_location(sample) diff --git a/test/test_database_merger.py b/test/test_database_merger.py index f3e8b0472..79743f904 100644 --- a/test/test_database_merger.py +++ b/test/test_database_merger.py @@ -40,7 +40,7 @@ def __init__(self): @with_tree def pick_and_place_plan(self): with simulated_robot: - ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() MoveTorsoAction([0.3]).resolve().perform() pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve() pickup_arm = pickup_pose.reachable_arms[0] @@ -59,7 +59,7 @@ def pick_and_place_plan(self): PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() - ParkArmsAction.Action(Arms.BOTH).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() class MergerTestCaseBase(unittest.TestCase): diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py index 4c7d10822..e03f6d5db 100644 --- a/test/test_database_resolver.py +++ b/test/test_database_resolver.py @@ -5,10 +5,11 @@ import pycram.plan_failures from pycram import task from pycram.bullet_world import BulletWorld, Object -from pycram.designators import action_designator, object_designator +from pycram.designators import action_designator from pycram.designators.actions.actions import MoveTorsoActionPerformable, PickUpActionPerformable, \ NavigateActionPerformable from pycram.orm.base import Base +from pycram.designators.object_designator import ObjectDesignatorDescription from pycram.process_module import ProcessModule from pycram.process_module import simulated_robot from pycram.pose import Pose @@ -48,26 +49,28 @@ def setUpClass(cls) -> None: cls.robot = Object(robot_description.name, ObjectType.ROBOT, robot_description.name + ".urdf") ProcessModule.execution_delay = False cls.engine = sqlalchemy.create_engine(pycrorm_uri) - cls.session = sqlalchemy.orm.Session(bind=cls.engine) def setUp(self) -> None: self.world.reset_bullet_world() pycram.orm.base.Base.metadata.create_all(self.engine) + self.session = sqlalchemy.orm.Session(bind=self.engine) self.session.commit() def tearDown(self) -> None: self.world.reset_bullet_world() pycram.task.reset_tree() + pycram.orm.base.ProcessMetaData.reset() + self.session.rollback() + pycram.orm.base.Base.metadata.drop_all(self.engine) + self.session.close() @classmethod def tearDownClass(cls) -> None: cls.world.exit() - cls.session.commit() - cls.session.close() @with_tree def plan(self): - object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + object_description = ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) with simulated_robot: NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() @@ -86,10 +89,9 @@ def test_costmap_no_obstacles(self): with simulated_robot: # action_designator.NavigateAction.Action(sample.pose).perform() - action_designator.MoveTorsoAction.Action(sample.torso_height).perform() - PickUpActionPerformable( - object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() + MoveTorsoActionPerformable(sample.torso_height).perform() + PickUpActionPerformable(ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, + grasp=sample.grasp).perform() @unittest.skip def test_costmap_with_obstacles(self): @@ -104,11 +106,12 @@ def test_costmap_with_obstacles(self): MoveTorsoActionPerformable(sample.torso_height).perform() try: PickUpActionPerformable( - object_designator.ObjectDesignatorDescription(types=["milk"]).resolve(), + ObjectDesignatorDescription(types=["milk"]).resolve(), arm=sample.reachable_arm, grasp=sample.grasp).perform() except pycram.plan_failures.PlanFailure: continue return + kitchen.remove() raise pycram.plan_failures.PlanFailure() diff --git a/test/test_orm.py b/test/test_orm.py index 0a70b99b4..8d7293b09 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -1,9 +1,7 @@ import os import unittest - -import sqlalchemy +from sqlalchemy import select import sqlalchemy.orm - import pycram.orm.action_designator import pycram.orm.base import pycram.orm.motion_designator @@ -26,7 +24,7 @@ class ORMTestSchema(unittest.TestCase): - engine: sqlalchemy.engine.Engine + engine: sqlalchemy.engine session: sqlalchemy.orm.Session @classmethod @@ -99,51 +97,45 @@ def tearDown(self): pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() - def test_node(self): """Test if the objects in the database is equal with the objects that got serialized.""" self.plan() pycram.orm.base.ProcessMetaData().description = "Unittest" pycram.task.task_tree.root.insert(self.session, ) - node_results = self.session.query(pycram.orm.task.TaskTreeNode).all() + node_results = self.session.scalars(select(pycram.orm.task.TaskTreeNode)).all() self.assertEqual(len(node_results), len(pycram.task.task_tree.root)) - code_results = self.session.query(pycram.orm.task.Code).all() + code_results = self.session.scalars(select(pycram.orm.task.Code)).all() self.assertEqual(len(code_results), len(pycram.task.task_tree.root)) - position_results = self.session.query(pycram.orm.base.Position).all() + position_results = self.session.scalars(select(pycram.orm.base.Position)).all() self.assertEqual(14, len(position_results)) - quaternion_results = self.session.query(pycram.orm.base.Quaternion).all() + quaternion_results = self.session.scalars(select(pycram.orm.base.Quaternion)).all() self.assertEqual(14, len(quaternion_results)) - park_arms_results = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + park_arms_results = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() self.assertEqual(0, len(park_arms_results)) - navigate_results = self.session.query(pycram.orm.action_designator.NavigateAction).all() + navigate_results = self.session.scalars(select(pycram.orm.action_designator.NavigateAction)).all() self.assertEqual(1, len(navigate_results)) - action_results = self.session.query(pycram.orm.action_designator.Action).all() + action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() self.assertEqual(4, len(action_results)) def test_metadata_existence(self): pycram.orm.base.ProcessMetaData().description = "metadata_existence_test" self.plan() pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.base.Pose).all() + result = self.session.scalars(select(pycram.orm.base.Pose)).all() self.assertTrue(all([r.process_metadata is not None for r in result])) def test_task_tree_node_parents(self): self.plan() pycram.orm.base.ProcessMetaData().description = "task_tree_node_parents_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.task.TaskTreeNode).all() + result = self.session.scalars(select(pycram.orm.task.TaskTreeNode)).all() self.assertTrue([result[i].parent == result[result[i].parent_id - 1] for i in range(len(result)) if result[i].parent_id is not None]) @@ -151,23 +143,23 @@ def test_meta_data(self): self.plan() pycram.orm.base.ProcessMetaData().description = "Unittest" pycram.task.task_tree.root.insert(self.session, ) - metadata_results = self.session.query(pycram.orm.base.ProcessMetaData).all() + metadata_results = self.session.scalars(select(pycram.orm.base.ProcessMetaData)).all() self.assertEqual(1, len(metadata_results)) - action_results = self.session.query(pycram.orm.action_designator.Action).all() + action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() self.assertTrue(all([a.process_metadata_id for a in action_results])) - park_arms_results = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + park_arms_results = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() self.assertTrue(all([a.process_metadata_id for a in park_arms_results])) - object_results = self.session.query(pycram.orm.object_designator.Object).all() + object_results = self.session.scalars(select(pycram.orm.object_designator.Object)).all() self.assertTrue(all([o.process_metadata_id for o in object_results])) def test_meta_data_alternation(self): self.plan() pycram.orm.base.ProcessMetaData().description = "meta_data_alternation_test" pycram.task.task_tree.root.insert(self.session, ) - metadata_result = self.session.query(pycram.orm.base.ProcessMetaData).first() + metadata_result = self.session.scalars(select(pycram.orm.base.ProcessMetaData)).first() self.assertEqual(metadata_result.description, "meta_data_alternation_test") @@ -187,14 +179,14 @@ def test_pose(self): self.plan() pycram.orm.base.ProcessMetaData().description = "pose_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.base.Pose).all() + result = self.session.scalars(select(pycram.orm.base.Pose)).all() self.assertTrue(all([r.position is not None and r.orientation is not None for r in result])) def test_pose_mixin(self): self.plan() pycram.orm.base.ProcessMetaData().description = "pose_mixin_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.base.RobotState).all() + result = self.session.scalars(select(pycram.orm.base.RobotState)).all() self.assertTrue(all([r.pose is not None and r.pose_id == r.pose.id for r in result])) @@ -242,8 +234,8 @@ def test_plan_serialization(self): pycram.orm.base.ProcessMetaData().description = "Unittest" tt = pycram.task.task_tree tt.insert(self.session) - action_results = self.session.query(pycram.orm.action_designator.Action).all() - motion_results = self.session.query(pycram.orm.motion_designator.Motion).all() + action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() + motion_results = self.session.scalars(select(pycram.orm.motion_designator.Motion)).all() self.assertEqual(len(tt) - 2, len(action_results) + len(motion_results)) @@ -270,19 +262,13 @@ def tearDown(self): pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() - def test_code_designator_type(self): action = NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])) with simulated_robot: action.perform() pycram.orm.base.ProcessMetaData().description = "code_designator_type_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.task.Code).filter(pycram.orm.task.Code.function == "perform").all() + result = self.session.scalars(select(pycram.orm.task.Code).where(pycram.orm.task.Code.function == "perform")).all() self.assertEqual(result[0].designator.dtype, action_designator.NavigateAction.__name__) self.assertEqual(result[1].designator.dtype, motion_designator.MoveMotion.__name__) @@ -292,7 +278,7 @@ def test_parkArmsAction(self): action.perform() pycram.orm.base.ProcessMetaData().description = "parkArmsAction_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.action_designator.ParkArmsAction).all() + result = self.session.scalars(select(pycram.orm.action_designator.ParkArmsAction)).all() self.assertTrue(all([result[i+1].dtype is not pycram.orm.action_designator.Action.dtype if result[i].dtype is pycram.orm.action_designator.ParkArmsAction.dtype else None for i in range(len(result)-1)])) @@ -305,8 +291,8 @@ def test_transportAction(self): action.perform() pycram.orm.base.ProcessMetaData().description = "transportAction_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.action_designator.TransportAction).all() - milk_object = self.session.query(pycram.orm.object_designator.Object).first() + result = self.session.scalars(select(pycram.orm.action_designator.TransportAction)).all() + milk_object = self.session.scalars(select(pycram.orm.object_designator.Object)).first() self.assertEqual(milk_object.pose, result[0].object.pose) def test_lookAt_and_detectAction(self): @@ -319,7 +305,7 @@ def test_lookAt_and_detectAction(self): action.perform() pycram.orm.base.ProcessMetaData().description = "detectAction_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.action_designator.DetectAction).all() + result = self.session.scalars(select(pycram.orm.action_designator.DetectAction)).all() self.assertEqual(result[0].object.name, "milk") def test_setGripperAction(self): @@ -328,7 +314,7 @@ def test_setGripperAction(self): action.perform() pycram.orm.base.ProcessMetaData().description = "setGripperAction_test" pycram.task.task_tree.root.insert(self.session) - result = self.session.query(pycram.orm.action_designator.SetGripperAction).all() + result = self.session.scalars(select(pycram.orm.action_designator.SetGripperAction)).all() self.assertEqual(result[0].gripper, "left") self.assertEqual(result[0].motion, "open") @@ -348,8 +334,8 @@ def test_open_and_closeAction(self): pycram.orm.base.ProcessMetaData().description = "open_and_closeAction_test" pycram.task.task_tree.root.insert(self.session) - open_result = self.session.query(pycram.orm.action_designator.OpenAction).all() - close_result = self.session.query(pycram.orm.action_designator.CloseAction).all() + open_result = self.session.scalars(select(pycram.orm.action_designator.OpenAction)).all() + close_result = self.session.scalars(select(pycram.orm.action_designator.CloseAction)).all() self.assertTrue(open_result is not None) self.assertEqual(open_result[0].object.name, "handle_cab10_t") self.assertTrue(close_result is not None) From 4081685b100771fa2cf7d75bb47d41a7519cf637 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Mon, 22 Jan 2024 18:20:31 +0100 Subject: [PATCH 08/12] [test] fixed an issue where the orm session was not properly clossed after test --- test/test_orm.py | 32 +++----------------------------- 1 file changed, 3 insertions(+), 29 deletions(-) diff --git a/test/test_orm.py b/test/test_orm.py index 8d7293b09..e4c2a7f25 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -190,14 +190,13 @@ def test_pose_mixin(self): self.assertTrue(all([r.pose is not None and r.pose_id == r.pose.id for r in result])) -class ORMObjectDesignatorTestCase(BulletWorldTestCase): +class ORMObjectDesignatorTestCase(ORMTestSchema, BulletWorldTestCase): """Test ORM functionality with a plan including object designators. """ engine: sqlalchemy.engine.Engine session: sqlalchemy.orm.Session - @with_tree - def plan(self): + def test_plan_serialization(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) self.assertEqual(description.ground().object_designator.name, "milk") @@ -206,37 +205,12 @@ def plan(self): MoveTorsoActionPerformable(0.3).perform() PickUpActionPerformable(object_description.resolve(), "left", "front").perform() description.resolve().perform() - - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - cls.session = sqlalchemy.orm.Session(bind=cls.engine) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session.commit() - - def tearDown(self): - super().tearDown() - pycram.orm.base.Base.metadata.drop_all(self.engine) - pycram.task.reset_tree() - - @classmethod - def tearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() - - def test_plan_serialization(self): - self.plan() pycram.orm.base.ProcessMetaData().description = "Unittest" tt = pycram.task.task_tree tt.insert(self.session) action_results = self.session.scalars(select(pycram.orm.action_designator.Action)).all() motion_results = self.session.scalars(select(pycram.orm.motion_designator.Motion)).all() - self.assertEqual(len(tt) - 2, len(action_results) + len(motion_results)) + self.assertEqual(len(tt) - 1, len(action_results) + len(motion_results)) class ORMActionDesignatorTestCase(BulletWorldTestCase): From f6dd146402985fe9eb1473754a57bd3a6c11ae84 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Fri, 26 Jan 2024 12:21:53 +0100 Subject: [PATCH 09/12] [test] fixed orm test inheritance pattern --- test/bullet_world_testcase.py | 2 + test/test_orm.py | 69 ++++++++--------------------------- 2 files changed, 18 insertions(+), 53 deletions(-) diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py index b34b434c4..8687a294a 100644 --- a/test/bullet_world_testcase.py +++ b/test/bullet_world_testcase.py @@ -3,6 +3,7 @@ import numpy as np import rospkg +import pycram.task from pycram.bullet_world import BulletWorld, Object, fix_missing_inertial from pycram.pose import Pose from pycram.robot_descriptions import robot_description @@ -34,6 +35,7 @@ def setUp(self): def tearDown(self): self.world.reset_bullet_world() + pycram.task.reset_tree() @classmethod def tearDownClass(cls): diff --git a/test/test_orm.py b/test/test_orm.py index e4c2a7f25..4c5063fc7 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -23,7 +23,7 @@ from pycram.task import with_tree -class ORMTestSchema(unittest.TestCase): +class DatabaseTestCaseMixin(BulletWorldTestCase): engine: sqlalchemy.engine session: sqlalchemy.orm.Session @@ -45,12 +45,8 @@ def tearDown(self): pycram.orm.base.Base.metadata.drop_all(self.engine) self.session.close() - @classmethod - def TearDownClass(cls): - super().tearDownClass() - cls.session.commit() - cls.session.close() +class ORMTestSchemaTestCase(DatabaseTestCaseMixin, unittest.TestCase): def test_schema_creation(self): pycram.orm.base.Base.metadata.create_all(self.engine) self.session.commit() @@ -75,27 +71,18 @@ def test_schema_creation(self): self.assertTrue("CloseAction" in tables) -class ORMTaskTreeTestCase(test_task_tree.TaskTreeTestCase): - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() +class ORMTaskTreeTestCase(DatabaseTestCaseMixin): - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() + @with_tree + def plan(self): + object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) + description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) + self.assertEqual(description.ground().object_designator.name, "milk") + with simulated_robot: + NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() + MoveTorsoActionPerformable(0.3).perform() + PickUpActionPerformable(object_description.resolve(), "left", "front").perform() + description.resolve().perform() def test_node(self): """Test if the objects in the database is equal with the objects that got serialized.""" @@ -163,7 +150,7 @@ def test_meta_data_alternation(self): self.assertEqual(metadata_result.description, "meta_data_alternation_test") -class MixinTestCase(ORMTestSchema, BulletWorldTestCase): +class MixinTestCase(DatabaseTestCaseMixin): @with_tree def plan(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) @@ -190,12 +177,9 @@ def test_pose_mixin(self): self.assertTrue(all([r.pose is not None and r.pose_id == r.pose.id for r in result])) -class ORMObjectDesignatorTestCase(ORMTestSchema, BulletWorldTestCase): +class ORMObjectDesignatorTestCase(DatabaseTestCaseMixin): """Test ORM functionality with a plan including object designators. """ - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - def test_plan_serialization(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) @@ -213,28 +197,7 @@ def test_plan_serialization(self): self.assertEqual(len(tt) - 1, len(action_results) + len(motion_results)) -class ORMActionDesignatorTestCase(BulletWorldTestCase): - - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - - @classmethod - def setUpClass(cls): - super().setUpClass() - cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) - - def setUp(self): - super().setUp() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() - - def tearDown(self): - super().tearDown() - pycram.task.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() +class ORMActionDesignatorTestCase(DatabaseTestCaseMixin): def test_code_designator_type(self): action = NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])) From 5e32afdb9ba57a9d6befec2c78087fee1f5e5d9b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Mon, 29 Jan 2024 16:34:45 +0100 Subject: [PATCH 10/12] [designator] removed deprecated motions --- src/pycram/designators/motion_designator.py | 62 --------------- .../process_modules/boxy_process_modules.py | 59 -------------- .../default_process_modules.py | 52 ------------- .../process_modules/pr2_process_modules.py | 77 ------------------- 4 files changed, 250 deletions(-) diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 44b131aa5..31c163e0f 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -118,68 +118,6 @@ def insert(self, session, *args, **kwargs) -> ORMMoveMotion: return motion -@dataclass -class PickUpMotion(BaseMotion): - """ - Lets the robot pick up a specific object - """ - - object_desig: ObjectDesignatorDescription.Object - """ - Object designator describing the object to be picked up - """ - arm: str - """ - Arm that should be used for picking up the object - """ - grasp: str - """ - From which direction the object should be grasped, e.g. 'left', 'front', etc. - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.pick_up().execute(self) - - def to_sql(self) -> ORMMotionDesignator: - pass - - def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: - pass - - -@dataclass -class PlaceMotion(BaseMotion): - """ - Lets the robot place an object that was picked up - """ - - object: ObjectDesignatorDescription.Object - """ - Object designator of the object to be placed - """ - target: Pose - """ - Pose at which the object should be placed - """ - arm: str - """ - Arm that is currently holding the object - """ - - @with_tree - def perform(self): - pm_manager = ProcessModuleManager.get_manager() - return pm_manager.place().execute(self) - - def to_sql(self) -> ORMMotionDesignator: - pass - - def insert(self, session: Session, *args, **kwargs) -> ORMMotionDesignator: - pass - - @dataclass class MoveTCPMotion(BaseMotion): """ diff --git a/src/pycram/process_modules/boxy_process_modules.py b/src/pycram/process_modules/boxy_process_modules.py index a455e6743..9ecd818f0 100644 --- a/src/pycram/process_modules/boxy_process_modules.py +++ b/src/pycram/process_modules/boxy_process_modules.py @@ -37,55 +37,6 @@ def _execute(self, desig: MoveMotion): robot.set_pose(desig.target) -class BoxyPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class BoxyPlace(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() - local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose(object_pose, - robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) - target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() - - _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) - - class BoxyOpen(ProcessModule): """ Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. @@ -251,8 +202,6 @@ class BoxyManager(ProcessModuleManager): def __init__(self): super().__init__("boxy") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -267,14 +216,6 @@ def navigate(self): if ProcessModuleManager.execution_type == "simulated": return BoxyNavigation(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return BoxyPickUp(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return BoxyPlace(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return BoxyMoveHead(self._looking_lock) diff --git a/src/pycram/process_modules/default_process_modules.py b/src/pycram/process_modules/default_process_modules.py index 9705722a4..87bea5cb8 100644 --- a/src/pycram/process_modules/default_process_modules.py +++ b/src/pycram/process_modules/default_process_modules.py @@ -23,48 +23,6 @@ def _execute(self, desig: MoveMotion): robot.set_pose(desig.target) -class DefaultPickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class DefaultPlace(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - _move_arm_tcp(desig.target, robot, arm) - robot.detach(object) - - class DefaultMoveHead(ProcessModule): """ This process module moves the head to look at a specific point in the world coordinate frame. @@ -228,8 +186,6 @@ class DefaultManager(ProcessModuleManager): def __init__(self): super().__init__("default") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -244,14 +200,6 @@ def navigate(self): if ProcessModuleManager.execution_type == "simulated": return DefaultNavigation(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return DefaultPickUp(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return DefaultPlace(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return DefaultMoveHead(self._looking_lock) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 6bdc8d3df..77cc067eb 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -55,54 +55,6 @@ def _execute(self, desig: MoveMotion): robot.set_pose(desig.target) -class Pr2PickUp(ProcessModule): - """ - This process module is for picking up a given object. - The object has to be reachable for this process module to succeed. - """ - - def _execute(self, desig: PickUpMotion): - object = desig.object_desig.bullet_world_object - robot = BulletWorld.robot - grasp = robot_description.grasps.get_orientation_for_grasp(desig.grasp) - target = object.get_pose() - target.orientation.x = grasp[0] - target.orientation.y = grasp[1] - target.orientation.z = grasp[2] - target.orientation.w = grasp[3] - - arm = desig.arm - - _move_arm_tcp(target, robot, arm) - tool_frame = robot_description.get_tool_frame(arm) - robot.attach(object, tool_frame) - - -class Pr2Place(ProcessModule): - """ - This process module places an object at the given position in world coordinate frame. - """ - - def _execute(self, desig: PlaceMotion): - """ - - :param desig: A PlaceMotion - :return: - """ - object = desig.object.bullet_world_object - robot = BulletWorld.robot - arm = desig.arm - - # Transformations such that the target position is the position of the object and not the tcp - object_pose = object.get_pose() - local_tf = LocalTransformer() - tcp_to_object = local_tf.transform_pose(object_pose, robot.get_link_tf_frame(robot_description.get_tool_frame(arm))) - target_diff = desig.target.to_transform("target").inverse_times(tcp_to_object.to_transform("object")).to_pose() - - _move_arm_tcp(target_diff, robot, arm) - robot.detach(object) - - class Pr2MoveHead(ProcessModule): """ This process module moves the head to look at a specific point in the world coordinate frame. @@ -271,18 +223,6 @@ def _execute(self, designator: MoveMotion) -> Any: giskard.achieve_cartesian_goal(designator.target, robot_description.base_link, "map") -class Pr2PickUpReal(ProcessModule): - - def _execute(self, designator: PickUpMotion) -> Any: - pass - - -class Pr2PlaceReal(ProcessModule): - - def _execute(self, designator: BaseMotion) -> Any: - pass - - class Pr2MoveHeadReal(ProcessModule): """ Process module for the real robot to move that such that it looks at the given position. Uses the same calculation @@ -339,9 +279,6 @@ def _execute(self, designator: DetectingMotion) -> Any: return bullet_obj[0] - - - class Pr2MoveTCPReal(ProcessModule): """ Moves the tool center point of the real PR2 while avoiding all collisions @@ -434,8 +371,6 @@ class Pr2Manager(ProcessModuleManager): def __init__(self): super().__init__("pr2") self._navigate_lock = Lock() - self._pick_up_lock = Lock() - self._place_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() self._move_tcp_lock = Lock() @@ -452,18 +387,6 @@ def navigate(self): elif ProcessModuleManager.execution_type == "real": return Pr2NavigationReal(self._navigate_lock) - def pick_up(self): - if ProcessModuleManager.execution_type == "simulated": - return Pr2PickUp(self._pick_up_lock) - elif ProcessModuleManager.execution_type == "real": - return Pr2PickUpReal(self._pick_up_lock) - - def place(self): - if ProcessModuleManager.execution_type == "simulated": - return Pr2Place(self._place_lock) - elif ProcessModuleManager.execution_type == "real": - return Pr2PlaceReal(self._place_lock) - def looking(self): if ProcessModuleManager.execution_type == "simulated": return Pr2MoveHead(self._looking_lock) From c9328d5e41e58d874ce356973e89013702560122 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Thu, 1 Feb 2024 15:58:43 +0100 Subject: [PATCH 11/12] [doc] updated orm querying, motions and actions in documentation --- doc/source/designators.rst | 2 +- doc/source/index.rst | 48 +- examples/cram_plan_tutorial.ipynb | 18 +- examples/custom_resolver.ipynb | 70 +- examples/intro.ipynb | 2265 +++++++++++++------------- examples/local_transformer.ipynb | 97 +- examples/migrate_neems.ipynb | 4 +- examples/motion_designator.ipynb | 296 ++-- examples/orm_example.ipynb | 206 ++- examples/orm_querying_examples.ipynb | 57 +- 10 files changed, 1582 insertions(+), 1481 deletions(-) diff --git a/doc/source/designators.rst b/doc/source/designators.rst index d07af0a5e..08ec4d8f4 100644 --- a/doc/source/designators.rst +++ b/doc/source/designators.rst @@ -45,7 +45,7 @@ action looks like this: .. code-block:: python - NavigateAction.Action(robot_position=((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)), target_location=[[1, 0, 0], [0, 0, 0, 1]]) + NavigateActionPerformable(robot_position=((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)), target_location=[[1, 0, 0], [0, 0, 0, 1]]) A visual representation of the whole idea of designator and designator descriptions can be diff --git a/doc/source/index.rst b/doc/source/index.rst index c8ab43941..fb1161d2d 100644 --- a/doc/source/index.rst +++ b/doc/source/index.rst @@ -52,30 +52,48 @@ The code for this plan can be seen below. .. code-block:: python - @with_simulated_robot - def plan(): - MotionDesignator(MoveArmJointsMotionDescription(left_arm_config='park', - right_arm_config='park')).perform() + from pycram.bullet_world import BulletWorld, Object + from pycram.process_module import simulated_robot + from pycram.designators.motion_designator import * + from pycram.designators.location_designator import * + from pycram.designators.action_designator import * + from pycram.designators.object_designator import * + from pycram.enums import ObjectType - MotionDesignator(MoveMotionDescription(target=moving_targets[robot_name]["sink"][0], - orientation=moving_targets[robot_name]["sink"][1])).perform() + world = BulletWorld() + kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") + robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") + cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", position=[1.4, 1, 0.95]) - det_obj = MotionDesignator(DetectingMotionDescription(object_type="milk")).perform() + cereal_desig = ObjectDesignatorDescription(names=["cereal"]) + kitchen_desig = ObjectDesignatorDescription(names=["kitchen"]) + robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() - MotionDesignator(PickUpMotionDescription(object=milk, arm="left", grasp="front")).perform() + with simulated_robot: + ParkArmsAction([Arms.BOTH]).resolve().perform() + MoveTorsoAction([0.3]).resolve().perform() - MotionDesignator(MoveMotionDescription(target=moving_targets[robot_name]["island"][0], - orientation=moving_targets[robot_name]["island"][1])).perform() + pickup_pose = CostmapLocation(target=cereal_desig.resolve(), reachable_for=robot_desig).resolve() + pickup_arm = pickup_pose.reachable_arms[0] - MotionDesignator(PlaceMotionDescription(object=milk, target=[-0.9, 1, 0.93], arm="left")).perform() + NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() - MotionDesignator(MoveArmJointsMotionDescription(left_arm_config='park', - right_arm_config='park')).perform() + PickUpAction(object_designator_description=cereal_desig, arms=[pickup_arm], grasps=["front"]).resolve().perform() - MotionDesignator(MoveMotionDescription(target=[0.0, 0.0, 0], - orientation=[0, 0, 0, 1])).perform() + ParkArmsAction([Arms.BOTH]).resolve().perform() + place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_desig.resolve(), cereal_desig.resolve()).resolve() + + place_stand = CostmapLocation(place_island.pose, reachable_for=robot_desig, reachable_arm=pickup_arm).resolve() + + NavigateAction(target_locations=[place_stand.pose]).resolve().perform() + + PlaceAction(cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform() + + ParkArmsAction([Arms.BOTH]).resolve().perform() + + world.exit() Tutorials --------- diff --git a/examples/cram_plan_tutorial.ipynb b/examples/cram_plan_tutorial.ipynb index a7fd04383..a26cd5b88 100644 --- a/examples/cram_plan_tutorial.ipynb +++ b/examples/cram_plan_tutorial.ipynb @@ -287,30 +287,30 @@ "def plan(obj, obj_desig, torso=0.2, place=\"countertop\"):\n", " world.reset_bullet_world()\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", - " MoveTorsoAction.Action(torso).perform()\n", + " MoveTorsoActionPerformable(torso).perform()\n", " location = CostmapLocation(target=obj_desig, reachable_for=robot_desig)\n", " pose = location.resolve()\n", " print()\n", - " NavigateAction.Action(pose.pose).perform()\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " NavigateActionPerformable(pose.pose).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " good_torsos.append(torso)\n", " picked_up_arm = pose.reachable_arms[0]\n", - " PickUpAction.Action(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=\"front\").perform()\n", + " PickUpActionPerformable(object_designator=obj_desig, arm=pose.reachable_arms[0], grasp=\"front\").perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", " scm = SemanticCostmapLocation(place, apartment_desig, obj_desig)\n", " pose_island = scm.resolve()\n", "\n", " place_location = CostmapLocation(target=pose_island.pose, reachable_for=robot_desig, reachable_arm=picked_up_arm)\n", " pose = place_location.resolve()\n", "\n", - " NavigateAction.Action(pose.pose).perform()\n", + " NavigateActionPerformable(pose.pose).perform()\n", "\n", - " PlaceAction.Action(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()\n", + " PlaceActionPerformable(object_designator=obj_desig, target_location=pose_island.pose, arm=picked_up_arm).perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsActionPerformable(Arms.BOTH).perform()\n", "\n", "good_torsos = []\n", "for obj_name in object_names:\n", diff --git a/examples/custom_resolver.ipynb b/examples/custom_resolver.ipynb index 21e1bf7cc..ef4413897 100644 --- a/examples/custom_resolver.ipynb +++ b/examples/custom_resolver.ipynb @@ -23,8 +23,8 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:35.037185202Z", - "start_time": "2024-01-05T11:39:32.842006673Z" + "end_time": "2024-01-29T16:05:37.443271489Z", + "start_time": "2024-01-29T16:05:35.328010988Z" } }, "outputs": [ @@ -32,22 +32,15 @@ "name": "stdout", "output_type": "stream", "text": [ - "Collecting probabilistic_model\r\n", - " Using cached probabilistic_model-1.5.18-py3-none-any.whl (30 kB)\r\n", - "Requirement already satisfied, skipping upgrade: portion>=2.4.1 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.4.1)\r\n", + "Requirement already up-to-date: probabilistic_model in /home/dprueser/.local/lib/python3.8/site-packages (1.6.20)\r\n", "Requirement already satisfied, skipping upgrade: random-events>=1.2.5 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (1.2.5)\r\n", - "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", "Requirement already satisfied, skipping upgrade: anytree>=2.9.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.9.0)\r\n", + "Requirement already satisfied, skipping upgrade: portion>=2.4.1 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (2.4.1)\r\n", + "Requirement already satisfied, skipping upgrade: plotly>=5.18.0 in /home/dprueser/.local/lib/python3.8/site-packages (from probabilistic_model) (5.18.0)\r\n", + "Requirement already satisfied, skipping upgrade: six in /usr/local/lib/python3.8/dist-packages (from anytree>=2.9.0->probabilistic_model) (1.16.0)\r\n", "Requirement already satisfied, skipping upgrade: sortedcontainers~=2.2 in /home/dprueser/.local/lib/python3.8/site-packages (from portion>=2.4.1->probabilistic_model) (2.4.0)\r\n", "Requirement already satisfied, skipping upgrade: tenacity>=6.2.0 in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (8.2.3)\r\n", - "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n", - "Requirement already satisfied, skipping upgrade: six in /usr/lib/python3/dist-packages (from anytree>=2.9.0->probabilistic_model) (1.14.0)\r\n", - "Installing collected packages: probabilistic-model\r\n", - " Attempting uninstall: probabilistic-model\r\n", - " Found existing installation: probabilistic-model 1.5.17\r\n", - " Uninstalling probabilistic-model-1.5.17:\r\n", - " Successfully uninstalled probabilistic-model-1.5.17\r\n", - "Successfully installed probabilistic-model-1.5.18\r\n" + "Requirement already satisfied, skipping upgrade: packaging in /home/dprueser/.local/lib/python3.8/site-packages (from plotly>=5.18.0->probabilistic_model) (23.2)\r\n" ] } ], @@ -60,8 +53,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:36.023993906Z", - "start_time": "2024-01-05T11:39:35.047597162Z" + "end_time": "2024-01-29T16:05:38.648889392Z", + "start_time": "2024-01-29T16:05:37.442437872Z" } }, "outputs": [ @@ -74,8 +67,8 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1704454775.649694]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1704454775.654652]: Failed to import Giskard messages\n" + "[WARN] [1706544338.060994]: Failed to import Giskard messages\n", + "[WARN] [1706544338.065403]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], @@ -249,8 +242,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2024-01-05T11:41:21.098968043Z", - "start_time": "2024-01-05T11:39:36.024974459Z" + "end_time": "2024-01-29T16:07:21.351710589Z", + "start_time": "2024-01-29T16:05:38.643086881Z" } }, "outputs": [ @@ -270,9 +263,9 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - " 38%|███▊ | 369/960 [00:40<01:06, 8.92it/s, success_rate=0.0867]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/l_gripper_motor_screw_link (parent map) at time 1704454816.991220 according to authority default_authority\n", + " 96%|█████████▌| 923/960 [01:38<00:03, 9.52it/s, success_rate=0.0845]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame spoon_2 (parent map) at time 1706544437.280444 according to authority default_authority\n", " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", - "100%|██████████| 960/960 [01:44<00:00, 9.47it/s, success_rate=0.0854]" + "100%|██████████| 960/960 [01:42<00:00, 10.12it/s, success_rate=0.0854]" ] } ], @@ -298,8 +291,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:41:21.193149641Z", - "start_time": "2024-01-05T11:41:21.142076887Z" + "end_time": "2024-01-29T16:07:21.445516208Z", + "start_time": "2024-01-29T16:07:21.396046276Z" } }, "outputs": [], @@ -325,8 +318,8 @@ "metadata": { "scrolled": true, "ExecuteTime": { - "end_time": "2024-01-05T11:41:21.278910069Z", - "start_time": "2024-01-05T11:41:21.194743363Z" + "end_time": "2024-01-29T16:07:21.541375696Z", + "start_time": "2024-01-29T16:07:21.448402324Z" } }, "outputs": [], @@ -365,8 +358,8 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2024-01-05T11:41:21.326300043Z", - "start_time": "2024-01-05T11:41:21.284443546Z" + "end_time": "2024-01-29T16:07:21.605599589Z", + "start_time": "2024-01-29T16:07:21.552928567Z" } }, "outputs": [ @@ -374,7 +367,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"TaskTreeNode\".status AS \"TaskTreeNode_status\", \"Object\".type AS \"Object_type\", \"PickUpAction\".arm AS \"PickUpAction_arm\", \"PickUpAction\".grasp AS \"PickUpAction_grasp\", \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", + "SELECT \"TaskTreeNode\".status, \"Object\".type, \"PickUpAction\".arm, \"PickUpAction\".grasp, \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" AS \"Pose_1\" ON \"Pose_1\".id = \"RobotState\".pose_id JOIN \"Position\" AS \"Position_2\" ON \"Position_2\".id = \"Pose_1\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id JOIN \"Pose\" AS \"Pose_2\" ON \"Pose_2\".id = \"Object\".pose_id JOIN \"Position\" AS \"Position_1\" ON \"Position_1\".id = \"Pose_2\".position_id\n", " status type arm grasp \\\n", "0 FAILED ObjectType.BREAKFAST_CEREAL left left \n", @@ -407,6 +400,7 @@ } ], "source": [ + "from sqlalchemy import select\n", "from pycram.orm.base import Pose as ORMPose\n", "\n", "robot_pose = sqlalchemy.orm.aliased(ORMPose)\n", @@ -414,7 +408,7 @@ "robot_pos = sqlalchemy.orm.aliased(Position)\n", "object_pos = sqlalchemy.orm.aliased(Position)\n", "\n", - "query = (session.query(TaskTreeNode.status, Object.type, ORMPickUpAction.arm, ORMPickUpAction.grasp,\n", + "query = (select(TaskTreeNode.status, Object.type, ORMPickUpAction.arm, ORMPickUpAction.grasp,\n", " sqlalchemy.label(\"relative torso height\", object_pos.z - RobotState.torso_height),\n", " sqlalchemy.label(\"x\", robot_pos.x - object_pos.x), \n", " sqlalchemy.label(\"y\", robot_pos.y - object_pos.y))\n", @@ -428,7 +422,7 @@ " .join(object_pos, object_pose.position))\n", "print(query)\n", "\n", - "df = pd.read_sql(query.statement, session.get_bind())\n", + "df = pd.read_sql(query, session.get_bind())\n", "df[\"status\"] = df[\"status\"].apply(lambda x: str(x.name))\n", "print(df)" ] @@ -445,8 +439,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:41:21.658478914Z", - "start_time": "2024-01-05T11:41:21.326092672Z" + "end_time": "2024-01-29T16:07:21.939517021Z", + "start_time": "2024-01-29T16:07:21.601075366Z" } }, "outputs": [ @@ -454,7 +448,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "100%|██████████| 960/960 [01:45<00:00, 9.11it/s, success_rate=0.0854]\n" + "100%|██████████| 960/960 [01:43<00:00, 9.32it/s, success_rate=0.0854]\n" ] }, { @@ -487,8 +481,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:46:59.969239637Z", - "start_time": "2024-01-05T11:41:21.659115424Z" + "end_time": "2024-01-29T16:13:01.579305735Z", + "start_time": "2024-01-29T16:07:21.942920319Z" } }, "outputs": [ @@ -499,11 +493,11 @@ "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", diff --git a/examples/intro.ipynb b/examples/intro.ipynb index d0247e613..9096f380c 100644 --- a/examples/intro.ipynb +++ b/examples/intro.ipynb @@ -12,8 +12,8 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:17.693331699Z", - "start_time": "2024-01-05T11:38:17.084580161Z" + "end_time": "2024-01-29T16:14:50.953932170Z", + "start_time": "2024-01-29T16:14:50.284068023Z" } }, "outputs": [ @@ -26,8 +26,8 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1704454697.683303]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1704454697.688683]: Failed to import Giskard messages\n" + "[WARN] [1706544890.889466]: Failed to import Giskard messages\n", + "[WARN] [1706544890.894063]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], @@ -53,8 +53,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:18.005042043Z", - "start_time": "2024-01-05T11:38:17.754902302Z" + "end_time": "2024-01-29T16:14:51.215354072Z", + "start_time": "2024-01-29T16:14:50.980844838Z" } }, "outputs": [ @@ -92,19 +92,11 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:18.052419130Z", - "start_time": "2024-01-05T11:38:18.021916002Z" + "end_time": "2024-01-29T16:14:51.251936758Z", + "start_time": "2024-01-29T16:14:51.234948638Z" } }, "outputs": [ - { - "data": { - "text/plain": "[array([[[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n ...,\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]]], dtype=uint8),\n array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n ...,\n [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n 0.80473447],\n [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n 0.8031688 ],\n [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n 0.80160314]], dtype=float32),\n array([[-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n ...,\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - }, { "name": "stderr", "output_type": "stream", @@ -112,6 +104,14 @@ "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" ] + }, + { + "data": { + "text/plain": "[array([[[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n [[255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n ...,\n [255, 255, 255, 255],\n [255, 255, 255, 255],\n [255, 255, 255, 255]],\n \n ...,\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]],\n \n [[239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n ...,\n [239, 239, 239, 255],\n [239, 239, 239, 255],\n [239, 239, 239, 255]]], dtype=uint8),\n array([[0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n [0.99999994, 0.99999994, 0.99999994, ..., 0.99999994, 0.99999994,\n 0.99999994],\n ...,\n [0.80473447, 0.80473447, 0.80473447, ..., 0.80473447, 0.80473447,\n 0.80473447],\n [0.8031688 , 0.8031688 , 0.8031688 , ..., 0.8031688 , 0.8031688 ,\n 0.8031688 ],\n [0.80160314, 0.80160314, 0.80160314, ..., 0.80160314, 0.80160314,\n 0.80160314]], dtype=float32),\n array([[-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n [-1, -1, -1, ..., -1, -1, -1],\n ...,\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1],\n [ 1, 1, 1, ..., 1, 1, 1]], dtype=int32)]" + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" } ], "source": [ @@ -149,8 +149,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:18.094930935Z", - "start_time": "2024-01-05T11:38:18.028522514Z" + "end_time": "2024-01-29T16:14:51.319150211Z", + "start_time": "2024-01-29T16:14:51.238519616Z" } }, "outputs": [], @@ -172,8 +172,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:18.097339564Z", - "start_time": "2024-01-05T11:38:18.096109982Z" + "end_time": "2024-01-29T16:14:51.361155485Z", + "start_time": "2024-01-29T16:14:51.360054431Z" } }, "outputs": [], @@ -193,8 +193,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:18.142227637Z", - "start_time": "2024-01-05T11:38:18.099015288Z" + "end_time": "2024-01-29T16:14:51.404210573Z", + "start_time": "2024-01-29T16:14:51.360232825Z" } }, "outputs": [], @@ -214,8 +214,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:21.716481380Z", - "start_time": "2024-01-05T11:38:18.142151725Z" + "end_time": "2024-01-29T16:14:54.380977290Z", + "start_time": "2024-01-29T16:14:51.404113811Z" } }, "outputs": [ @@ -261,8 +261,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:22.274205765Z", - "start_time": "2024-01-05T11:38:21.717433092Z" + "end_time": "2024-01-29T16:14:54.951307507Z", + "start_time": "2024-01-29T16:14:54.380864026Z" } }, "outputs": [], @@ -276,8 +276,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:30.651191441Z", - "start_time": "2024-01-05T11:38:30.646880498Z" + "end_time": "2024-01-29T16:15:01.756580723Z", + "start_time": "2024-01-29T16:14:54.961615686Z" } }, "outputs": [], @@ -290,8 +290,8 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:33.983967866Z", - "start_time": "2024-01-05T11:38:30.649221674Z" + "end_time": "2024-01-29T16:15:05.092885577Z", + "start_time": "2024-01-29T16:15:05.091325419Z" } }, "outputs": [], @@ -312,8 +312,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:34.747939331Z", - "start_time": "2024-01-05T11:38:33.985076157Z" + "end_time": "2024-01-29T16:15:05.847569295Z", + "start_time": "2024-01-29T16:15:05.093669437Z" } }, "outputs": [], @@ -326,8 +326,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:34.845540496Z", - "start_time": "2024-01-05T11:38:34.749095455Z" + "end_time": "2024-01-29T16:15:05.944506368Z", + "start_time": "2024-01-29T16:15:05.849514274Z" } }, "outputs": [], @@ -350,8 +350,8 @@ "execution_count": 13, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:35.719367991Z", - "start_time": "2024-01-05T11:38:34.846943933Z" + "end_time": "2024-01-29T16:15:06.810575822Z", + "start_time": "2024-01-29T16:15:05.955432369Z" } }, "outputs": [], @@ -364,8 +364,8 @@ "execution_count": 14, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:36.074271101Z", - "start_time": "2024-01-05T11:38:36.067925005Z" + "end_time": "2024-01-29T16:15:07.161867271Z", + "start_time": "2024-01-29T16:15:07.159806241Z" } }, "outputs": [], @@ -385,8 +385,8 @@ "execution_count": 15, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:36.075213164Z", - "start_time": "2024-01-05T11:38:36.070266632Z" + "end_time": "2024-01-29T16:15:07.193879719Z", + "start_time": "2024-01-29T16:15:07.161787791Z" } }, "outputs": [], @@ -399,8 +399,8 @@ "execution_count": 16, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:38.237916925Z", - "start_time": "2024-01-05T11:38:36.073992561Z" + "end_time": "2024-01-29T16:15:09.283523209Z", + "start_time": "2024-01-29T16:15:07.188128850Z" } }, "outputs": [], @@ -413,8 +413,8 @@ "execution_count": 17, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:39.305574111Z", - "start_time": "2024-01-05T11:38:38.238541874Z" + "end_time": "2024-01-29T16:15:10.347228575Z", + "start_time": "2024-01-29T16:15:09.280179794Z" } }, "outputs": [], @@ -444,8 +444,8 @@ "execution_count": 18, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:41.970422232Z", - "start_time": "2024-01-05T11:38:39.305851886Z" + "end_time": "2024-01-29T16:15:12.961385558Z", + "start_time": "2024-01-29T16:15:10.347682457Z" } }, "outputs": [ @@ -453,6 +453,10 @@ "name": "stderr", "output_type": "stream", "text": [ + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", @@ -479,21 +483,11 @@ "metadata": { "scrolled": true, "ExecuteTime": { - "end_time": "2024-01-05T11:38:42.271284276Z", - "start_time": "2024-01-05T11:38:41.989603716Z" + "end_time": "2024-01-29T16:15:13.246942739Z", + "start_time": "2024-01-29T16:15:12.963076402Z" } }, "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", - "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" - ] - }, { "name": "stdout", "output_type": "stream", @@ -513,8 +507,8 @@ "execution_count": 20, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:42.355865055Z", - "start_time": "2024-01-05T11:38:42.270253375Z" + "end_time": "2024-01-29T16:15:13.334740244Z", + "start_time": "2024-01-29T16:15:13.249042978Z" } }, "outputs": [ @@ -539,8 +533,8 @@ "execution_count": 21, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:42.450970248Z", - "start_time": "2024-01-05T11:38:42.358883797Z" + "end_time": "2024-01-29T16:15:13.430296220Z", + "start_time": "2024-01-29T16:15:13.334507309Z" } }, "outputs": [ @@ -586,8 +580,8 @@ "execution_count": 22, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:42.957994018Z", - "start_time": "2024-01-05T11:38:42.494000112Z" + "end_time": "2024-01-29T16:15:13.939910370Z", + "start_time": "2024-01-29T16:15:13.437727375Z" } }, "outputs": [], @@ -598,7 +592,7 @@ "description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", "\n", "with simulated_robot:\n", - " description.resolve().perform()\n", + " description.perform()\n", " \n" ] }, @@ -608,8 +602,8 @@ "metadata": { "scrolled": true, "ExecuteTime": { - "end_time": "2024-01-05T11:38:43.465280115Z", - "start_time": "2024-01-05T11:38:42.966741953Z" + "end_time": "2024-01-29T16:15:14.446281217Z", + "start_time": "2024-01-29T16:15:13.948689644Z" } }, "outputs": [], @@ -618,7 +612,7 @@ "\n", "@with_simulated_robot\n", "def move():\n", - " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).resolve().perform()\n", + " MoveMotion(target=Pose([0, 0, 0], [0, 0, 0, 1])).perform()\n", "\n", "move()" ] @@ -628,8 +622,6 @@ "metadata": {}, "source": [ "Other implemented Motion Designator descriptions are:\n", - "* Pick up\n", - "* Place\n", "* Accessing\n", "* Move TCP\n", "* Looking\n", @@ -653,14 +645,14 @@ "execution_count": 24, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:43.472245439Z", - "start_time": "2024-01-05T11:38:43.467689247Z" + "end_time": "2024-01-29T16:15:14.453344058Z", + "start_time": "2024-01-29T16:15:14.448819583Z" } }, "outputs": [ { "data": { - "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1704454719\n nsecs: 314873933\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1704454722\n nsecs: 356852293\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1704454719\n nsecs: 314873933\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1704454722\n nsecs: 356852293\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...)>)" + "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...)>)" }, "execution_count": 24, "metadata": {}, @@ -688,8 +680,43 @@ "metadata": { "scrolled": false, "ExecuteTime": { - "end_time": "2024-01-05T11:38:50.957195391Z", - "start_time": "2024-01-05T11:38:43.471315489Z" + "end_time": "2024-01-29T16:15:14.493010865Z", + "start_time": "2024-01-29T16:15:14.451251059Z" + } + }, + "outputs": [ + { + "data": { + "text/plain": "BelieveObject.Object(name='Milk', type=, bullet_world_object=Object(world=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...), _pose=, \nlocal_transformer=, \nname=Milk, \ntype=ObjectType.MILK, \ncolor=[1, 1, 1, 1], \nid=4, \npath=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \njoints: ..., \nlinks: ..., \nattachments: ..., \ncids: ..., \noriginal_pose=header: \n seq: 0\n stamp: \n secs: 1706544910\n nsecs: 372853994\n frame_id: \"map\"\npose: \n position: \n x: 1.0\n y: 0.0\n z: 1.0\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \ntf_frame=Milk_4, \nurdf_object: ..., \n_current_pose=header: \n seq: 0\n stamp: \n secs: 1706544913\n nsecs: 335583925\n frame_id: \"map\"\npose: \n position: \n x: 0.6\n y: -0.5\n z: 0.7\n orientation: \n x: 0.0\n y: 0.0\n z: 0.0\n w: 1.0, \n_current_link_poses: ..., \n_current_link_transforms: ..., \n_current_joint_states={}, \nbase_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \nlink_to_geometry: ...)>)" + }, + "execution_count": 25, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "from pycram.designators.object_designator import *\n", + "\n", + "milk_desig = BelieveObject(names=[\"Milk\"])\n", + "milk_desig.resolve()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Location Designators\n", + "Location Designators can create a position in cartesian space from a symbolic description." + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": { + "scrolled": false, + "ExecuteTime": { + "end_time": "2024-01-29T16:15:22.177685226Z", + "start_time": "2024-01-29T16:15:14.482072615Z" } }, "outputs": [ @@ -700,8 +727,8 @@ "Resolved: CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 189191341\n", + " secs: 1706544915\n", + " nsecs: 201824188\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -716,8 +743,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 901956796\n", + " secs: 1706544915\n", + " nsecs: 927674055\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -732,8 +759,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 911677598\n", + " secs: 1706544915\n", + " nsecs: 938544273\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -748,8 +775,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 921111106\n", + " secs: 1706544915\n", + " nsecs: 949182987\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -764,8 +791,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 930423736\n", + " secs: 1706544915\n", + " nsecs: 959582567\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -780,8 +807,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 939850091\n", + " secs: 1706544915\n", + " nsecs: 969888687\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -796,8 +823,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 949112415\n", + " secs: 1706544915\n", + " nsecs: 979798555\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -812,8 +839,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 958681583\n", + " secs: 1706544915\n", + " nsecs: 994071245\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -828,8 +855,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 968044519\n", + " secs: 1706544916\n", + " nsecs: 9548187\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -844,8 +871,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 977337121\n", + " secs: 1706544916\n", + " nsecs: 24910449\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -860,8 +887,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454724\n", - " nsecs: 986764192\n", + " secs: 1706544916\n", + " nsecs: 40581226\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -876,8 +903,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 5548715\n", + " secs: 1706544916\n", + " nsecs: 71025371\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -892,8 +919,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 24160146\n", + " secs: 1706544916\n", + " nsecs: 103567123\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -908,8 +935,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 33551692\n", + " secs: 1706544916\n", + " nsecs: 120177507\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -924,8 +951,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 43041944\n", + " secs: 1706544916\n", + " nsecs: 135888099\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -940,8 +967,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 52456855\n", + " secs: 1706544916\n", + " nsecs: 147926568\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -956,8 +983,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 61823606\n", + " secs: 1706544916\n", + " nsecs: 158867120\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -972,8 +999,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 71311473\n", + " secs: 1706544916\n", + " nsecs: 171792984\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -988,8 +1015,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 80672740\n", + " secs: 1706544916\n", + " nsecs: 183446645\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1004,8 +1031,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 90170145\n", + " secs: 1706544916\n", + " nsecs: 195348024\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1020,8 +1047,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 118748664\n", + " secs: 1706544916\n", + " nsecs: 225474596\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1036,8 +1063,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 128309965\n", + " secs: 1706544916\n", + " nsecs: 235300779\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1052,8 +1079,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 137932062\n", + " secs: 1706544916\n", + " nsecs: 246283054\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1068,8 +1095,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 147601604\n", + " secs: 1706544916\n", + " nsecs: 255914211\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1084,8 +1111,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 157281398\n", + " secs: 1706544916\n", + " nsecs: 265790700\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1100,8 +1127,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 166773319\n", + " secs: 1706544916\n", + " nsecs: 276698589\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1116,8 +1143,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 176612615\n", + " secs: 1706544916\n", + " nsecs: 289893150\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1132,8 +1159,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 186205387\n", + " secs: 1706544916\n", + " nsecs: 303444147\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1148,8 +1175,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 196010589\n", + " secs: 1706544916\n", + " nsecs: 314954519\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1164,8 +1191,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 205993652\n", + " secs: 1706544916\n", + " nsecs: 332621335\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1180,8 +1207,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 215711593\n", + " secs: 1706544916\n", + " nsecs: 348136901\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1196,8 +1223,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 225684404\n", + " secs: 1706544916\n", + " nsecs: 359711885\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1212,8 +1239,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 235526561\n", + " secs: 1706544916\n", + " nsecs: 370139122\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1228,8 +1255,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 255168437\n", + " secs: 1706544916\n", + " nsecs: 395307302\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1244,8 +1271,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 264625310\n", + " secs: 1706544916\n", + " nsecs: 405689239\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1260,8 +1287,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 274528026\n", + " secs: 1706544916\n", + " nsecs: 416645288\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1276,8 +1303,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 293281793\n", + " secs: 1706544916\n", + " nsecs: 437982320\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1292,8 +1319,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 302706003\n", + " secs: 1706544916\n", + " nsecs: 448891639\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1308,8 +1335,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 321676969\n", + " secs: 1706544916\n", + " nsecs: 469527959\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1324,8 +1351,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 331218719\n", + " secs: 1706544916\n", + " nsecs: 480023622\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1340,8 +1367,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 341029405\n", + " secs: 1706544916\n", + " nsecs: 490493774\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1356,8 +1383,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 350405693\n", + " secs: 1706544916\n", + " nsecs: 501381158\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1372,8 +1399,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 360239028\n", + " secs: 1706544916\n", + " nsecs: 511650800\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1388,8 +1415,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 369774341\n", + " secs: 1706544916\n", + " nsecs: 521751880\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1404,8 +1431,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 379291534\n", + " secs: 1706544916\n", + " nsecs: 532479286\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1420,8 +1447,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 408546209\n", + " secs: 1706544916\n", + " nsecs: 561986207\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1436,8 +1463,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 418382167\n", + " secs: 1706544916\n", + " nsecs: 571810960\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1452,8 +1479,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 428440093\n", + " secs: 1706544916\n", + " nsecs: 581784725\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1468,8 +1495,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 448060989\n", + " secs: 1706544916\n", + " nsecs: 601855516\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1484,8 +1511,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 458268404\n", + " secs: 1706544916\n", + " nsecs: 611686706\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1500,8 +1527,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 468119144\n", + " secs: 1706544916\n", + " nsecs: 622118949\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1516,8 +1543,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 477586746\n", + " secs: 1706544916\n", + " nsecs: 632574319\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1532,8 +1559,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 487447023\n", + " secs: 1706544916\n", + " nsecs: 643788337\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1548,8 +1575,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 496778726\n", + " secs: 1706544916\n", + " nsecs: 654880285\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1564,8 +1591,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 515697717\n", + " secs: 1706544916\n", + " nsecs: 675857067\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1580,8 +1607,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 535202741\n", + " secs: 1706544916\n", + " nsecs: 696542024\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1596,8 +1623,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 562966585\n", + " secs: 1706544916\n", + " nsecs: 728210210\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1612,8 +1639,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 603397607\n", + " secs: 1706544916\n", + " nsecs: 769653797\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1628,8 +1655,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 613683938\n", + " secs: 1706544916\n", + " nsecs: 779698371\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1644,8 +1671,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 623685836\n", + " secs: 1706544916\n", + " nsecs: 790320158\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1660,8 +1687,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 635320425\n", + " secs: 1706544916\n", + " nsecs: 800973892\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1676,8 +1703,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 645336151\n", + " secs: 1706544916\n", + " nsecs: 811413764\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1692,8 +1719,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 665030956\n", + " secs: 1706544916\n", + " nsecs: 832077503\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1708,8 +1735,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 684486150\n", + " secs: 1706544916\n", + " nsecs: 853376865\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1724,8 +1751,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 694182634\n", + " secs: 1706544916\n", + " nsecs: 865315675\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1740,8 +1767,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 703764915\n", + " secs: 1706544916\n", + " nsecs: 875989675\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1756,8 +1783,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 722873687\n", + " secs: 1706544916\n", + " nsecs: 896036386\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1772,8 +1799,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 732309579\n", + " secs: 1706544916\n", + " nsecs: 906243562\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1788,8 +1815,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 742166519\n", + " secs: 1706544916\n", + " nsecs: 916708469\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1804,8 +1831,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 752015113\n", + " secs: 1706544916\n", + " nsecs: 927308559\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1820,8 +1847,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 762117147\n", + " secs: 1706544916\n", + " nsecs: 937535762\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1836,8 +1863,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 772179365\n", + " secs: 1706544916\n", + " nsecs: 947962045\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1852,8 +1879,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 781810283\n", + " secs: 1706544916\n", + " nsecs: 958276748\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1868,8 +1895,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 792211532\n", + " secs: 1706544916\n", + " nsecs: 969069242\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1884,8 +1911,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 802207708\n", + " secs: 1706544916\n", + " nsecs: 980567216\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1900,8 +1927,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 811917543\n", + " secs: 1706544916\n", + " nsecs: 992730140\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1916,8 +1943,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 822474241\n", + " secs: 1706544917\n", + " nsecs: 3598690\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1932,8 +1959,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 832451105\n", + " secs: 1706544917\n", + " nsecs: 14332056\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1948,8 +1975,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 842451095\n", + " secs: 1706544917\n", + " nsecs: 24833202\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1964,8 +1991,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 864063024\n", + " secs: 1706544917\n", + " nsecs: 44901132\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1980,8 +2007,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 874287605\n", + " secs: 1706544917\n", + " nsecs: 55425643\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -1996,8 +2023,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 884132862\n", + " secs: 1706544917\n", + " nsecs: 66057682\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2012,8 +2039,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 893823862\n", + " secs: 1706544917\n", + " nsecs: 78454971\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2028,8 +2055,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 913730621\n", + " secs: 1706544917\n", + " nsecs: 98734617\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2044,8 +2071,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 923520088\n", + " secs: 1706544917\n", + " nsecs: 108744382\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2060,8 +2087,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 932972908\n", + " secs: 1706544917\n", + " nsecs: 119309425\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2076,8 +2103,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 942650079\n", + " secs: 1706544917\n", + " nsecs: 129229784\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2092,8 +2119,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 952360153\n", + " secs: 1706544917\n", + " nsecs: 139137268\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2108,8 +2135,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 962044715\n", + " secs: 1706544917\n", + " nsecs: 149181365\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2124,8 +2151,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454725\n", - " nsecs: 972100019\n", + " secs: 1706544917\n", + " nsecs: 159062862\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2140,8 +2167,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 10155916\n", + " secs: 1706544917\n", + " nsecs: 197809934\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2156,8 +2183,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 19874572\n", + " secs: 1706544917\n", + " nsecs: 207918167\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2172,8 +2199,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 39346933\n", + " secs: 1706544917\n", + " nsecs: 227355718\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2188,8 +2215,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 48823833\n", + " secs: 1706544917\n", + " nsecs: 237487077\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2204,8 +2231,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 58706760\n", + " secs: 1706544917\n", + " nsecs: 247480630\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2220,8 +2247,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 68709850\n", + " secs: 1706544917\n", + " nsecs: 257900953\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2236,8 +2263,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 87986469\n", + " secs: 1706544917\n", + " nsecs: 278165102\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2252,8 +2279,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 97659826\n", + " secs: 1706544917\n", + " nsecs: 288398027\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2268,8 +2295,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 107616424\n", + " secs: 1706544917\n", + " nsecs: 298751831\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2284,8 +2311,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 117786407\n", + " secs: 1706544917\n", + " nsecs: 308977603\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2300,8 +2327,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 127777814\n", + " secs: 1706544917\n", + " nsecs: 319071054\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2316,8 +2343,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 138854026\n", + " secs: 1706544917\n", + " nsecs: 329062938\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2332,8 +2359,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 149201869\n", + " secs: 1706544917\n", + " nsecs: 339415788\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2348,8 +2375,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 159495592\n", + " secs: 1706544917\n", + " nsecs: 349754333\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2364,8 +2391,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 169792652\n", + " secs: 1706544917\n", + " nsecs: 359853982\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2380,8 +2407,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 179657697\n", + " secs: 1706544917\n", + " nsecs: 371557712\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2396,8 +2423,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 189663410\n", + " secs: 1706544917\n", + " nsecs: 381916522\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2412,8 +2439,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 199556350\n", + " secs: 1706544917\n", + " nsecs: 391955852\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2428,8 +2455,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 209797859\n", + " secs: 1706544917\n", + " nsecs: 401677608\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2444,8 +2471,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 219633102\n", + " secs: 1706544917\n", + " nsecs: 411783218\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2460,8 +2487,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 229408979\n", + " secs: 1706544917\n", + " nsecs: 421791553\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2476,8 +2503,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 239880800\n", + " secs: 1706544917\n", + " nsecs: 432043552\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2492,8 +2519,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 250423669\n", + " secs: 1706544917\n", + " nsecs: 442192077\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2508,8 +2535,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 260511398\n", + " secs: 1706544917\n", + " nsecs: 452643394\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2524,8 +2551,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 280463457\n", + " secs: 1706544917\n", + " nsecs: 474348306\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2540,8 +2567,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 291324138\n", + " secs: 1706544917\n", + " nsecs: 484361648\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2556,8 +2583,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 313117504\n", + " secs: 1706544917\n", + " nsecs: 504082441\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2572,8 +2599,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 323770761\n", + " secs: 1706544917\n", + " nsecs: 514279603\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2588,8 +2615,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 334040164\n", + " secs: 1706544917\n", + " nsecs: 524206399\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2604,8 +2631,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 344825744\n", + " secs: 1706544917\n", + " nsecs: 534056901\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2620,8 +2647,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 355269432\n", + " secs: 1706544917\n", + " nsecs: 544548749\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2636,8 +2663,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 365238666\n", + " secs: 1706544917\n", + " nsecs: 554365396\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2652,8 +2679,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 376126766\n", + " secs: 1706544917\n", + " nsecs: 564265251\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2668,8 +2695,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 387210607\n", + " secs: 1706544917\n", + " nsecs: 574109077\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2684,8 +2711,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 408796787\n", + " secs: 1706544917\n", + " nsecs: 593857049\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2700,8 +2727,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 419713258\n", + " secs: 1706544917\n", + " nsecs: 604115009\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2716,8 +2743,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 439749002\n", + " secs: 1706544917\n", + " nsecs: 626145839\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2732,8 +2759,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 449327468\n", + " secs: 1706544917\n", + " nsecs: 636278867\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2748,8 +2775,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 470635890\n", + " secs: 1706544917\n", + " nsecs: 657084703\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2764,8 +2791,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 480293035\n", + " secs: 1706544917\n", + " nsecs: 667837142\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2780,8 +2807,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 500214099\n", + " secs: 1706544917\n", + " nsecs: 689748287\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2796,8 +2823,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 520314931\n", + " secs: 1706544917\n", + " nsecs: 709959506\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2812,8 +2839,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 530125856\n", + " secs: 1706544917\n", + " nsecs: 720077991\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2828,8 +2855,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 540076732\n", + " secs: 1706544917\n", + " nsecs: 729877233\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2844,8 +2871,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 549759149\n", + " secs: 1706544917\n", + " nsecs: 740610361\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2860,8 +2887,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 569632768\n", + " secs: 1706544917\n", + " nsecs: 760720729\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2876,8 +2903,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 589303016\n", + " secs: 1706544917\n", + " nsecs: 780767917\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2892,8 +2919,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 599044799\n", + " secs: 1706544917\n", + " nsecs: 791338205\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2908,8 +2935,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 609395503\n", + " secs: 1706544917\n", + " nsecs: 803500652\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2924,8 +2951,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 619220018\n", + " secs: 1706544917\n", + " nsecs: 816298246\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2940,8 +2967,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 629041433\n", + " secs: 1706544917\n", + " nsecs: 826271057\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2956,8 +2983,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 639055252\n", + " secs: 1706544917\n", + " nsecs: 836186170\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2972,8 +2999,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 648601055\n", + " secs: 1706544917\n", + " nsecs: 846061468\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -2988,8 +3015,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 668470382\n", + " secs: 1706544917\n", + " nsecs: 866801500\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3004,8 +3031,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 678457498\n", + " secs: 1706544917\n", + " nsecs: 879305124\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3020,8 +3047,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 688319921\n", + " secs: 1706544917\n", + " nsecs: 889095067\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3036,8 +3063,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 697925806\n", + " secs: 1706544917\n", + " nsecs: 899603128\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3052,8 +3079,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 708099126\n", + " secs: 1706544917\n", + " nsecs: 909643411\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3068,8 +3095,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 717890977\n", + " secs: 1706544917\n", + " nsecs: 919872760\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3084,8 +3111,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 727897167\n", + " secs: 1706544917\n", + " nsecs: 930837392\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3100,8 +3127,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 737935304\n", + " secs: 1706544917\n", + " nsecs: 940669298\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3116,8 +3143,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 747606992\n", + " secs: 1706544917\n", + " nsecs: 950444221\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3132,8 +3159,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 761164903\n", + " secs: 1706544917\n", + " nsecs: 960928440\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3148,8 +3175,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 772180557\n", + " secs: 1706544917\n", + " nsecs: 971531629\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3164,8 +3191,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 786565303\n", + " secs: 1706544917\n", + " nsecs: 981504678\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3180,8 +3207,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 800817966\n", + " secs: 1706544917\n", + " nsecs: 991544485\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3196,8 +3223,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 811119318\n", + " secs: 1706544918\n", + " nsecs: 1651048\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3212,8 +3239,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 821317911\n", + " secs: 1706544918\n", + " nsecs: 11669158\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3228,8 +3255,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 831474781\n", + " secs: 1706544918\n", + " nsecs: 21538734\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3244,8 +3271,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 851423263\n", + " secs: 1706544918\n", + " nsecs: 41085720\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3260,8 +3287,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 861536979\n", + " secs: 1706544918\n", + " nsecs: 50888299\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3276,8 +3303,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 871807813\n", + " secs: 1706544918\n", + " nsecs: 60572385\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3292,8 +3319,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 891721963\n", + " secs: 1706544918\n", + " nsecs: 79907894\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3308,8 +3335,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 901814699\n", + " secs: 1706544918\n", + " nsecs: 92547893\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3324,8 +3351,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 915273189\n", + " secs: 1706544918\n", + " nsecs: 102537870\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3340,8 +3367,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 926554679\n", + " secs: 1706544918\n", + " nsecs: 112884759\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3356,8 +3383,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 940182209\n", + " secs: 1706544918\n", + " nsecs: 123359680\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3372,8 +3399,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 965167760\n", + " secs: 1706544918\n", + " nsecs: 144013881\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3388,8 +3415,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 975308895\n", + " secs: 1706544918\n", + " nsecs: 153965234\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3404,8 +3431,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454726\n", - " nsecs: 985270500\n", + " secs: 1706544918\n", + " nsecs: 164059638\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3420,8 +3447,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 14838933\n", + " secs: 1706544918\n", + " nsecs: 194094181\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3436,8 +3463,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 24997949\n", + " secs: 1706544918\n", + " nsecs: 203748703\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3452,8 +3479,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 34455060\n", + " secs: 1706544918\n", + " nsecs: 213503837\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3468,8 +3495,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 44534921\n", + " secs: 1706544918\n", + " nsecs: 224017381\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3484,8 +3511,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 54246187\n", + " secs: 1706544918\n", + " nsecs: 233902931\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3500,8 +3527,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 83569049\n", + " secs: 1706544918\n", + " nsecs: 263562440\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3516,8 +3543,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 93582630\n", + " secs: 1706544918\n", + " nsecs: 273334980\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3532,8 +3559,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 112976551\n", + " secs: 1706544918\n", + " nsecs: 294827461\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3548,8 +3575,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 123248338\n", + " secs: 1706544918\n", + " nsecs: 304959297\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3564,8 +3591,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 132923603\n", + " secs: 1706544918\n", + " nsecs: 314709424\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3580,8 +3607,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 142979860\n", + " secs: 1706544918\n", + " nsecs: 325293064\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3596,8 +3623,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 152651309\n", + " secs: 1706544918\n", + " nsecs: 335991859\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3612,8 +3639,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 163275718\n", + " secs: 1706544918\n", + " nsecs: 346140861\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3628,8 +3655,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 173099279\n", + " secs: 1706544918\n", + " nsecs: 356065750\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3644,8 +3671,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 199396848\n", + " secs: 1706544918\n", + " nsecs: 377432346\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3660,8 +3687,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 220230817\n", + " secs: 1706544918\n", + " nsecs: 397984027\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3676,8 +3703,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 230001449\n", + " secs: 1706544918\n", + " nsecs: 408214569\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3692,8 +3719,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 240165948\n", + " secs: 1706544918\n", + " nsecs: 418574810\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3708,8 +3735,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 249710559\n", + " secs: 1706544918\n", + " nsecs: 428881645\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3724,8 +3751,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 260489702\n", + " secs: 1706544918\n", + " nsecs: 438925266\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3740,8 +3767,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 270841121\n", + " secs: 1706544918\n", + " nsecs: 449546337\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3756,8 +3783,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 280830144\n", + " secs: 1706544918\n", + " nsecs: 459606885\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3772,8 +3799,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 290997505\n", + " secs: 1706544918\n", + " nsecs: 469818115\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3788,8 +3815,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 300702810\n", + " secs: 1706544918\n", + " nsecs: 480182886\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3804,8 +3831,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 310925006\n", + " secs: 1706544918\n", + " nsecs: 490321397\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3820,8 +3847,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 320628404\n", + " secs: 1706544918\n", + " nsecs: 501243829\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3836,8 +3863,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 330796957\n", + " secs: 1706544918\n", + " nsecs: 511368751\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3852,8 +3879,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 350512981\n", + " secs: 1706544918\n", + " nsecs: 531534433\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3868,8 +3895,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 360772371\n", + " secs: 1706544918\n", + " nsecs: 542391061\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3884,8 +3911,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 370627880\n", + " secs: 1706544918\n", + " nsecs: 552552223\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3900,8 +3927,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 380810976\n", + " secs: 1706544918\n", + " nsecs: 562627077\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3916,8 +3943,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 390733242\n", + " secs: 1706544918\n", + " nsecs: 572496891\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3932,8 +3959,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 401291370\n", + " secs: 1706544918\n", + " nsecs: 582713603\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3948,8 +3975,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 411706209\n", + " secs: 1706544918\n", + " nsecs: 593607664\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3964,8 +3991,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 421663045\n", + " secs: 1706544918\n", + " nsecs: 604231595\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3980,8 +4007,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 431575298\n", + " secs: 1706544918\n", + " nsecs: 615185499\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -3996,8 +4023,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 442079544\n", + " secs: 1706544918\n", + " nsecs: 626099348\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4012,8 +4039,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 451843976\n", + " secs: 1706544918\n", + " nsecs: 635951280\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4028,8 +4055,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 471526861\n", + " secs: 1706544918\n", + " nsecs: 656157970\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4044,8 +4071,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 481531858\n", + " secs: 1706544918\n", + " nsecs: 666541576\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4060,8 +4087,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 510531187\n", + " secs: 1706544918\n", + " nsecs: 696901559\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4076,8 +4103,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 520304441\n", + " secs: 1706544918\n", + " nsecs: 706899642\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4092,8 +4119,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 529991149\n", + " secs: 1706544918\n", + " nsecs: 717074871\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4108,8 +4135,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 539878845\n", + " secs: 1706544918\n", + " nsecs: 727422475\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4124,8 +4151,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 549521207\n", + " secs: 1706544918\n", + " nsecs: 737593412\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4140,8 +4167,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 559333324\n", + " secs: 1706544918\n", + " nsecs: 748543024\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4156,8 +4183,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 568988084\n", + " secs: 1706544918\n", + " nsecs: 759326457\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4172,8 +4199,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 588930606\n", + " secs: 1706544918\n", + " nsecs: 780078649\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4188,8 +4215,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 598807811\n", + " secs: 1706544918\n", + " nsecs: 790158510\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4204,8 +4231,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 619289398\n", + " secs: 1706544918\n", + " nsecs: 812283277\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4220,8 +4247,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 629359006\n", + " secs: 1706544918\n", + " nsecs: 822262287\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4236,8 +4263,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 639499425\n", + " secs: 1706544918\n", + " nsecs: 832731246\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4252,8 +4279,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 649319171\n", + " secs: 1706544918\n", + " nsecs: 842995643\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4268,8 +4295,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 659330844\n", + " secs: 1706544918\n", + " nsecs: 853036403\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4284,8 +4311,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 669612884\n", + " secs: 1706544918\n", + " nsecs: 863647460\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4300,8 +4327,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 679676771\n", + " secs: 1706544918\n", + " nsecs: 873960256\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4316,8 +4343,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 689470529\n", + " secs: 1706544918\n", + " nsecs: 884471654\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4332,8 +4359,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 699056148\n", + " secs: 1706544918\n", + " nsecs: 895127058\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4348,8 +4375,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 708853006\n", + " secs: 1706544918\n", + " nsecs: 906480073\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4364,8 +4391,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 718634128\n", + " secs: 1706544918\n", + " nsecs: 917049169\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4380,8 +4407,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 728414297\n", + " secs: 1706544918\n", + " nsecs: 927591085\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4396,8 +4423,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 749159097\n", + " secs: 1706544918\n", + " nsecs: 947772502\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4412,8 +4439,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 759160280\n", + " secs: 1706544918\n", + " nsecs: 957824230\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4428,8 +4455,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 769248008\n", + " secs: 1706544918\n", + " nsecs: 968458414\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4444,8 +4471,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 779141187\n", + " secs: 1706544918\n", + " nsecs: 979157924\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4460,8 +4487,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 789060115\n", + " secs: 1706544918\n", + " nsecs: 989204168\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4476,8 +4503,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 818681716\n", + " secs: 1706544919\n", + " nsecs: 20420312\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4492,8 +4519,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 829430341\n", + " secs: 1706544919\n", + " nsecs: 31285047\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4508,8 +4535,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 839163064\n", + " secs: 1706544919\n", + " nsecs: 41488409\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4524,8 +4551,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 848954200\n", + " secs: 1706544919\n", + " nsecs: 51862955\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4540,8 +4567,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 859354734\n", + " secs: 1706544919\n", + " nsecs: 62264919\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4556,8 +4583,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 879795551\n", + " secs: 1706544919\n", + " nsecs: 82359075\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4572,8 +4599,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 899379730\n", + " secs: 1706544919\n", + " nsecs: 103036642\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4588,8 +4615,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 909643888\n", + " secs: 1706544919\n", + " nsecs: 113466262\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4604,8 +4631,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 929649591\n", + " secs: 1706544919\n", + " nsecs: 133843421\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4620,8 +4647,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 939815521\n", + " secs: 1706544919\n", + " nsecs: 144020557\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4636,8 +4663,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 959586143\n", + " secs: 1706544919\n", + " nsecs: 163890361\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4652,8 +4679,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 969409227\n", + " secs: 1706544919\n", + " nsecs: 173745393\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4668,8 +4695,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 979031085\n", + " secs: 1706544919\n", + " nsecs: 183910846\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4684,8 +4711,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 988930702\n", + " secs: 1706544919\n", + " nsecs: 194156885\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4700,8 +4727,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454727\n", - " nsecs: 999084234\n", + " secs: 1706544919\n", + " nsecs: 204169988\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4716,8 +4743,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 8918762\n", + " secs: 1706544919\n", + " nsecs: 214705228\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4732,8 +4759,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 28315067\n", + " secs: 1706544919\n", + " nsecs: 235476016\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4748,8 +4775,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 37947177\n", + " secs: 1706544919\n", + " nsecs: 245808124\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4764,8 +4791,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 47890901\n", + " secs: 1706544919\n", + " nsecs: 255650997\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4780,8 +4807,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 57522058\n", + " secs: 1706544919\n", + " nsecs: 265812635\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4796,8 +4823,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 67149162\n", + " secs: 1706544919\n", + " nsecs: 275789737\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4812,8 +4839,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 76910734\n", + " secs: 1706544919\n", + " nsecs: 286756038\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4828,8 +4855,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 87213277\n", + " secs: 1706544919\n", + " nsecs: 298326969\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4844,8 +4871,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 97060918\n", + " secs: 1706544919\n", + " nsecs: 308969020\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4860,8 +4887,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 108145713\n", + " secs: 1706544919\n", + " nsecs: 319038152\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4876,8 +4903,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 118573904\n", + " secs: 1706544919\n", + " nsecs: 329315423\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4892,8 +4919,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 128924608\n", + " secs: 1706544919\n", + " nsecs: 339347362\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4908,8 +4935,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 139441013\n", + " secs: 1706544919\n", + " nsecs: 349482059\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4924,8 +4951,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 149400472\n", + " secs: 1706544919\n", + " nsecs: 359558582\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4940,8 +4967,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 159833192\n", + " secs: 1706544919\n", + " nsecs: 369482517\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4956,8 +4983,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 169833660\n", + " secs: 1706544919\n", + " nsecs: 379415988\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4972,8 +4999,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 179687261\n", + " secs: 1706544919\n", + " nsecs: 389329671\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -4988,8 +5015,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 189734935\n", + " secs: 1706544919\n", + " nsecs: 399363040\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5004,8 +5031,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 199295997\n", + " secs: 1706544919\n", + " nsecs: 409301280\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5020,8 +5047,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 209303140\n", + " secs: 1706544919\n", + " nsecs: 419255733\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5036,8 +5063,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 219341278\n", + " secs: 1706544919\n", + " nsecs: 429352521\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5052,8 +5079,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 229027032\n", + " secs: 1706544919\n", + " nsecs: 439219951\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5068,8 +5095,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 238579750\n", + " secs: 1706544919\n", + " nsecs: 449213981\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5084,8 +5111,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 248189449\n", + " secs: 1706544919\n", + " nsecs: 459560871\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5100,8 +5127,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 258103370\n", + " secs: 1706544919\n", + " nsecs: 469874858\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5116,8 +5143,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 268151521\n", + " secs: 1706544919\n", + " nsecs: 479789495\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5132,8 +5159,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 278077602\n", + " secs: 1706544919\n", + " nsecs: 489479780\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5148,8 +5175,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 288434267\n", + " secs: 1706544919\n", + " nsecs: 499516248\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5164,8 +5191,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 298222303\n", + " secs: 1706544919\n", + " nsecs: 509546995\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5180,8 +5207,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 308148384\n", + " secs: 1706544919\n", + " nsecs: 519337415\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5196,8 +5223,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 329452991\n", + " secs: 1706544919\n", + " nsecs: 539964437\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5212,8 +5239,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 339732646\n", + " secs: 1706544919\n", + " nsecs: 550527572\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5228,8 +5255,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 349679470\n", + " secs: 1706544919\n", + " nsecs: 560472249\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5244,8 +5271,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 372056722\n", + " secs: 1706544919\n", + " nsecs: 580207109\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5260,8 +5287,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 382091522\n", + " secs: 1706544919\n", + " nsecs: 590407609\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5276,8 +5303,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 412238359\n", + " secs: 1706544919\n", + " nsecs: 621572971\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5292,8 +5319,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 422187566\n", + " secs: 1706544919\n", + " nsecs: 631641149\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5308,8 +5335,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 431981563\n", + " secs: 1706544919\n", + " nsecs: 641553401\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5324,8 +5351,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 441920280\n", + " secs: 1706544919\n", + " nsecs: 653319358\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5340,8 +5367,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 451804637\n", + " secs: 1706544919\n", + " nsecs: 663541078\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5356,8 +5383,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 475440740\n", + " secs: 1706544919\n", + " nsecs: 683467149\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5372,8 +5399,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 485437870\n", + " secs: 1706544919\n", + " nsecs: 693924903\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5388,8 +5415,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 495399475\n", + " secs: 1706544919\n", + " nsecs: 704140663\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5404,8 +5431,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 505185365\n", + " secs: 1706544919\n", + " nsecs: 713812112\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5420,8 +5447,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 515660047\n", + " secs: 1706544919\n", + " nsecs: 723740100\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5436,8 +5463,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 525471210\n", + " secs: 1706544919\n", + " nsecs: 733844518\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5452,8 +5479,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 536112785\n", + " secs: 1706544919\n", + " nsecs: 744749307\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5468,8 +5495,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 570803880\n", + " secs: 1706544919\n", + " nsecs: 774046421\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5484,8 +5511,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 580623388\n", + " secs: 1706544919\n", + " nsecs: 783934116\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5500,8 +5527,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 590690374\n", + " secs: 1706544919\n", + " nsecs: 794060468\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5516,8 +5543,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 600275516\n", + " secs: 1706544919\n", + " nsecs: 804785490\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5532,8 +5559,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 620325326\n", + " secs: 1706544919\n", + " nsecs: 825284957\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5548,8 +5575,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 639937162\n", + " secs: 1706544919\n", + " nsecs: 845585823\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5564,8 +5591,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 649961471\n", + " secs: 1706544919\n", + " nsecs: 856135606\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5580,8 +5607,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 669656515\n", + " secs: 1706544919\n", + " nsecs: 876502752\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5596,8 +5623,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 679559946\n", + " secs: 1706544919\n", + " nsecs: 886613845\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5612,8 +5639,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 689200401\n", + " secs: 1706544919\n", + " nsecs: 896639585\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5628,8 +5655,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 699025630\n", + " secs: 1706544919\n", + " nsecs: 907559156\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5644,8 +5671,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 718453407\n", + " secs: 1706544919\n", + " nsecs: 927424430\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5660,8 +5687,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 728411674\n", + " secs: 1706544919\n", + " nsecs: 937479496\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5676,8 +5703,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 747882127\n", + " secs: 1706544919\n", + " nsecs: 957934379\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5692,8 +5719,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 767148971\n", + " secs: 1706544919\n", + " nsecs: 977858066\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5708,8 +5735,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 777645587\n", + " secs: 1706544919\n", + " nsecs: 987823247\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5724,8 +5751,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 788507223\n", + " secs: 1706544919\n", + " nsecs: 997573375\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5740,8 +5767,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 798871278\n", + " secs: 1706544920\n", + " nsecs: 7592201\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5756,8 +5783,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 823041200\n", + " secs: 1706544920\n", + " nsecs: 27238607\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5772,8 +5799,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 833983421\n", + " secs: 1706544920\n", + " nsecs: 37187337\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5788,8 +5815,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 853881120\n", + " secs: 1706544920\n", + " nsecs: 57217121\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5804,8 +5831,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 863618850\n", + " secs: 1706544920\n", + " nsecs: 67085981\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5820,8 +5847,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 873755455\n", + " secs: 1706544920\n", + " nsecs: 77337503\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5836,8 +5863,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 883673667\n", + " secs: 1706544920\n", + " nsecs: 87342262\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5852,8 +5879,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 893738508\n", + " secs: 1706544920\n", + " nsecs: 97246408\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5868,8 +5895,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 903726816\n", + " secs: 1706544920\n", + " nsecs: 107070446\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5884,8 +5911,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 914144754\n", + " secs: 1706544920\n", + " nsecs: 117566823\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5900,8 +5927,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 924309968\n", + " secs: 1706544920\n", + " nsecs: 127794265\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5916,8 +5943,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 934568881\n", + " secs: 1706544920\n", + " nsecs: 137711286\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5932,8 +5959,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 944923162\n", + " secs: 1706544920\n", + " nsecs: 150225162\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5948,8 +5975,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 954938173\n", + " secs: 1706544920\n", + " nsecs: 160764455\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5964,8 +5991,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 964920282\n", + " secs: 1706544920\n", + " nsecs: 170743227\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5980,8 +6007,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 974496364\n", + " secs: 1706544920\n", + " nsecs: 180782556\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -5996,8 +6023,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454728\n", - " nsecs: 994848012\n", + " secs: 1706544920\n", + " nsecs: 201177597\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6012,8 +6039,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 5209684\n", + " secs: 1706544920\n", + " nsecs: 213806390\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6028,8 +6055,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 15120983\n", + " secs: 1706544920\n", + " nsecs: 223469972\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6044,8 +6071,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 34841537\n", + " secs: 1706544920\n", + " nsecs: 243908166\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6060,8 +6087,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 54306030\n", + " secs: 1706544920\n", + " nsecs: 263970375\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6076,8 +6103,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 63890218\n", + " secs: 1706544920\n", + " nsecs: 273994445\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6092,8 +6119,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 73581933\n", + " secs: 1706544920\n", + " nsecs: 284109830\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6108,8 +6135,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 83770036\n", + " secs: 1706544920\n", + " nsecs: 294022321\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6124,8 +6151,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 114562034\n", + " secs: 1706544920\n", + " nsecs: 326006412\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6140,8 +6167,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 128451824\n", + " secs: 1706544920\n", + " nsecs: 337008714\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6156,8 +6183,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 140075445\n", + " secs: 1706544920\n", + " nsecs: 347310543\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6172,8 +6199,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 149988651\n", + " secs: 1706544920\n", + " nsecs: 358036518\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6188,8 +6215,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 160300254\n", + " secs: 1706544920\n", + " nsecs: 370610475\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6204,8 +6231,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 170534372\n", + " secs: 1706544920\n", + " nsecs: 381522893\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6220,8 +6247,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 180233716\n", + " secs: 1706544920\n", + " nsecs: 391707181\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6236,8 +6263,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 190173625\n", + " secs: 1706544920\n", + " nsecs: 402300357\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6252,8 +6279,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 199840784\n", + " secs: 1706544920\n", + " nsecs: 412396192\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6268,8 +6295,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 209982872\n", + " secs: 1706544920\n", + " nsecs: 422230243\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6284,8 +6311,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 220086812\n", + " secs: 1706544920\n", + " nsecs: 432636737\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6300,8 +6327,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 230565071\n", + " secs: 1706544920\n", + " nsecs: 442850828\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6316,8 +6343,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 262809038\n", + " secs: 1706544920\n", + " nsecs: 473550319\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6332,8 +6359,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 274945259\n", + " secs: 1706544920\n", + " nsecs: 483459949\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6348,8 +6375,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 284489154\n", + " secs: 1706544920\n", + " nsecs: 493614435\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6364,8 +6391,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 294175386\n", + " secs: 1706544920\n", + " nsecs: 504096269\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6380,8 +6407,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 303731441\n", + " secs: 1706544920\n", + " nsecs: 513952732\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6396,8 +6423,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 313288927\n", + " secs: 1706544920\n", + " nsecs: 523840188\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6412,8 +6439,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 322869777\n", + " secs: 1706544920\n", + " nsecs: 533734798\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6428,8 +6455,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 332524299\n", + " secs: 1706544920\n", + " nsecs: 544054985\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6444,8 +6471,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 342247724\n", + " secs: 1706544920\n", + " nsecs: 553883552\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6460,8 +6487,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 351795196\n", + " secs: 1706544920\n", + " nsecs: 563676357\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6476,8 +6503,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 361424207\n", + " secs: 1706544920\n", + " nsecs: 573505640\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6492,8 +6519,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 371025085\n", + " secs: 1706544920\n", + " nsecs: 583408832\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6508,8 +6535,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 380615949\n", + " secs: 1706544920\n", + " nsecs: 593615531\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6524,8 +6551,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 399971723\n", + " secs: 1706544920\n", + " nsecs: 612910509\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6540,8 +6567,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 410209894\n", + " secs: 1706544920\n", + " nsecs: 622579574\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6556,8 +6583,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 420065879\n", + " secs: 1706544920\n", + " nsecs: 632315635\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6572,8 +6599,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 429817914\n", + " secs: 1706544920\n", + " nsecs: 642050027\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6588,8 +6615,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 439763307\n", + " secs: 1706544920\n", + " nsecs: 651688337\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6604,8 +6631,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 449737310\n", + " secs: 1706544920\n", + " nsecs: 661351203\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6620,8 +6647,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 459516048\n", + " secs: 1706544920\n", + " nsecs: 671063661\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6636,8 +6663,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 469855308\n", + " secs: 1706544920\n", + " nsecs: 680626392\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6652,8 +6679,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 480250120\n", + " secs: 1706544920\n", + " nsecs: 690305233\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6668,8 +6695,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 489985704\n", + " secs: 1706544920\n", + " nsecs: 699806213\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6684,8 +6711,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 514060497\n", + " secs: 1706544920\n", + " nsecs: 719159364\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6700,8 +6727,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 524521827\n", + " secs: 1706544920\n", + " nsecs: 728794574\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6716,8 +6743,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 534343481\n", + " secs: 1706544920\n", + " nsecs: 739019632\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6732,8 +6759,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 553424119\n", + " secs: 1706544920\n", + " nsecs: 758497953\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6748,8 +6775,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 563100337\n", + " secs: 1706544920\n", + " nsecs: 768382310\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6764,8 +6791,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 572806596\n", + " secs: 1706544920\n", + " nsecs: 778136730\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6780,8 +6807,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 592219591\n", + " secs: 1706544920\n", + " nsecs: 797852516\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6796,8 +6823,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 602131605\n", + " secs: 1706544920\n", + " nsecs: 807753801\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6812,8 +6839,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 612154722\n", + " secs: 1706544920\n", + " nsecs: 817374706\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6828,8 +6855,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 621884822\n", + " secs: 1706544920\n", + " nsecs: 827094316\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6844,8 +6871,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 631433486\n", + " secs: 1706544920\n", + " nsecs: 837395668\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6860,8 +6887,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 640997409\n", + " secs: 1706544920\n", + " nsecs: 848101377\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6876,8 +6903,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 689654350\n", + " secs: 1706544920\n", + " nsecs: 897163629\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6892,8 +6919,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 699763059\n", + " secs: 1706544920\n", + " nsecs: 907011747\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6908,8 +6935,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 709666728\n", + " secs: 1706544920\n", + " nsecs: 916730880\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6924,8 +6951,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 719478130\n", + " secs: 1706544920\n", + " nsecs: 926568746\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6940,8 +6967,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 729162454\n", + " secs: 1706544920\n", + " nsecs: 937285184\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6956,8 +6983,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 739078283\n", + " secs: 1706544920\n", + " nsecs: 947034358\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6972,8 +6999,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 748747825\n", + " secs: 1706544920\n", + " nsecs: 957026481\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -6988,8 +7015,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 758508443\n", + " secs: 1706544920\n", + " nsecs: 967617511\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7004,8 +7031,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 768118619\n", + " secs: 1706544920\n", + " nsecs: 977944612\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7020,8 +7047,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 778162002\n", + " secs: 1706544920\n", + " nsecs: 988276481\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7036,8 +7063,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 788316965\n", + " secs: 1706544920\n", + " nsecs: 998299598\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7052,8 +7079,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 798148393\n", + " secs: 1706544921\n", + " nsecs: 8562088\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7068,8 +7095,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 808482408\n", + " secs: 1706544921\n", + " nsecs: 18221139\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7084,8 +7111,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 818536281\n", + " secs: 1706544921\n", + " nsecs: 28006315\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7100,8 +7127,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 828765153\n", + " secs: 1706544921\n", + " nsecs: 37898063\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7116,8 +7143,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 848044395\n", + " secs: 1706544921\n", + " nsecs: 57746171\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7132,8 +7159,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 858117818\n", + " secs: 1706544921\n", + " nsecs: 67290544\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7148,8 +7175,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 867625951\n", + " secs: 1706544921\n", + " nsecs: 77006340\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7164,8 +7191,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 877413272\n", + " secs: 1706544921\n", + " nsecs: 86667060\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7180,8 +7207,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 887440681\n", + " secs: 1706544921\n", + " nsecs: 96265316\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7196,8 +7223,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 897084951\n", + " secs: 1706544921\n", + " nsecs: 105965852\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7212,8 +7239,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 907503843\n", + " secs: 1706544921\n", + " nsecs: 115576744\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7228,8 +7255,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 917495489\n", + " secs: 1706544921\n", + " nsecs: 125284671\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7244,8 +7271,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 927219152\n", + " secs: 1706544921\n", + " nsecs: 135140895\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7260,8 +7287,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 937005519\n", + " secs: 1706544921\n", + " nsecs: 144938230\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7276,8 +7303,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 946638345\n", + " secs: 1706544921\n", + " nsecs: 154725313\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7292,8 +7319,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 975183248\n", + " secs: 1706544921\n", + " nsecs: 183697938\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7308,8 +7335,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 984693288\n", + " secs: 1706544921\n", + " nsecs: 194594383\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7324,8 +7351,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454729\n", - " nsecs: 994870901\n", + " secs: 1706544921\n", + " nsecs: 204714298\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7340,8 +7367,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 4707336\n", + " secs: 1706544921\n", + " nsecs: 214483737\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7356,8 +7383,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 34533977\n", + " secs: 1706544921\n", + " nsecs: 244819402\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7372,8 +7399,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 44676065\n", + " secs: 1706544921\n", + " nsecs: 255036592\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7388,8 +7415,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 54926156\n", + " secs: 1706544921\n", + " nsecs: 265106439\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7404,8 +7431,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 64422369\n", + " secs: 1706544921\n", + " nsecs: 275486230\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7420,8 +7447,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 74395179\n", + " secs: 1706544921\n", + " nsecs: 285556316\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7436,8 +7463,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 83957433\n", + " secs: 1706544921\n", + " nsecs: 295557022\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7452,8 +7479,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 93839406\n", + " secs: 1706544921\n", + " nsecs: 305361986\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7468,8 +7495,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 103564739\n", + " secs: 1706544921\n", + " nsecs: 314964056\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7484,8 +7511,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 113248825\n", + " secs: 1706544921\n", + " nsecs: 324617624\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7500,8 +7527,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 123576402\n", + " secs: 1706544921\n", + " nsecs: 334374666\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7516,8 +7543,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 133637905\n", + " secs: 1706544921\n", + " nsecs: 344164848\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7532,8 +7559,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 143495321\n", + " secs: 1706544921\n", + " nsecs: 353968620\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7548,8 +7575,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 153331756\n", + " secs: 1706544921\n", + " nsecs: 363894939\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7564,8 +7591,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 163374900\n", + " secs: 1706544921\n", + " nsecs: 373680114\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7580,8 +7607,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 174808502\n", + " secs: 1706544921\n", + " nsecs: 383259534\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7596,8 +7623,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 185347795\n", + " secs: 1706544921\n", + " nsecs: 393896102\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7612,8 +7639,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 196193218\n", + " secs: 1706544921\n", + " nsecs: 403790950\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7628,8 +7655,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 206784486\n", + " secs: 1706544921\n", + " nsecs: 413312673\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7644,8 +7671,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 217048168\n", + " secs: 1706544921\n", + " nsecs: 422859430\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7660,8 +7687,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 227622985\n", + " secs: 1706544921\n", + " nsecs: 432488441\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7676,8 +7703,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 237950801\n", + " secs: 1706544921\n", + " nsecs: 442560195\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7692,8 +7719,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 248067378\n", + " secs: 1706544921\n", + " nsecs: 452620267\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7708,8 +7735,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 258363723\n", + " secs: 1706544921\n", + " nsecs: 462723493\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7724,8 +7751,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 268745660\n", + " secs: 1706544921\n", + " nsecs: 472694635\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7740,8 +7767,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 278952360\n", + " secs: 1706544921\n", + " nsecs: 482583999\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7756,8 +7783,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 289012908\n", + " secs: 1706544921\n", + " nsecs: 492875099\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7772,8 +7799,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 299065113\n", + " secs: 1706544921\n", + " nsecs: 503608465\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7788,8 +7815,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 339966058\n", + " secs: 1706544921\n", + " nsecs: 545543432\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7804,8 +7831,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 351265430\n", + " secs: 1706544921\n", + " nsecs: 556905508\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7820,8 +7847,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 383670091\n", + " secs: 1706544921\n", + " nsecs: 579474687\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7836,8 +7863,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 401098966\n", + " secs: 1706544921\n", + " nsecs: 590476512\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7852,8 +7879,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 414599180\n", + " secs: 1706544921\n", + " nsecs: 600560188\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7868,8 +7895,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 425669193\n", + " secs: 1706544921\n", + " nsecs: 611992359\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7884,8 +7911,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 436941385\n", + " secs: 1706544921\n", + " nsecs: 623099565\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7900,8 +7927,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 447655439\n", + " secs: 1706544921\n", + " nsecs: 633123397\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7916,8 +7943,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 470032453\n", + " secs: 1706544921\n", + " nsecs: 655219793\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7932,8 +7959,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 480431079\n", + " secs: 1706544921\n", + " nsecs: 665919780\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7948,8 +7975,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 490664720\n", + " secs: 1706544921\n", + " nsecs: 676297664\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7964,8 +7991,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 501055717\n", + " secs: 1706544921\n", + " nsecs: 686551332\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7980,8 +8007,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 511538267\n", + " secs: 1706544921\n", + " nsecs: 697294473\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -7996,8 +8023,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 521835565\n", + " secs: 1706544921\n", + " nsecs: 707584142\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8012,8 +8039,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 531538486\n", + " secs: 1706544921\n", + " nsecs: 717669010\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8028,8 +8055,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 540988683\n", + " secs: 1706544921\n", + " nsecs: 729904651\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8044,8 +8071,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 559922933\n", + " secs: 1706544921\n", + " nsecs: 755266666\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8060,8 +8087,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 569701671\n", + " secs: 1706544921\n", + " nsecs: 765379905\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8076,8 +8103,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 579053401\n", + " secs: 1706544921\n", + " nsecs: 775438785\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8092,8 +8119,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 588583230\n", + " secs: 1706544921\n", + " nsecs: 785464763\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8108,8 +8135,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 598399162\n", + " secs: 1706544921\n", + " nsecs: 797777175\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8124,8 +8151,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 608730077\n", + " secs: 1706544921\n", + " nsecs: 810024023\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8140,8 +8167,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 618813991\n", + " secs: 1706544921\n", + " nsecs: 821498632\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8156,8 +8183,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 628623723\n", + " secs: 1706544921\n", + " nsecs: 832209587\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8172,8 +8199,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 638496398\n", + " secs: 1706544921\n", + " nsecs: 842942476\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8188,8 +8215,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 648101329\n", + " secs: 1706544921\n", + " nsecs: 854440927\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8204,8 +8231,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 657718896\n", + " secs: 1706544921\n", + " nsecs: 864692211\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8220,8 +8247,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 667537689\n", + " secs: 1706544921\n", + " nsecs: 874951601\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8236,8 +8263,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 677129745\n", + " secs: 1706544921\n", + " nsecs: 885563135\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8252,8 +8279,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 686960220\n", + " secs: 1706544921\n", + " nsecs: 897155761\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8268,8 +8295,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 696655988\n", + " secs: 1706544921\n", + " nsecs: 907792329\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8284,8 +8311,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 706870079\n", + " secs: 1706544921\n", + " nsecs: 917936801\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8300,8 +8327,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 716971874\n", + " secs: 1706544921\n", + " nsecs: 928446054\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8316,8 +8343,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 746616125\n", + " secs: 1706544921\n", + " nsecs: 961369752\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8332,8 +8359,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 756603956\n", + " secs: 1706544921\n", + " nsecs: 973284721\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8348,8 +8375,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 766701936\n", + " secs: 1706544921\n", + " nsecs: 987850427\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8364,8 +8391,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 776644468\n", + " secs: 1706544921\n", + " nsecs: 998610258\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8380,8 +8407,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 786627054\n", + " secs: 1706544922\n", + " nsecs: 8282661\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8396,8 +8423,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 806035041\n", + " secs: 1706544922\n", + " nsecs: 27345895\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8412,8 +8439,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 815665483\n", + " secs: 1706544922\n", + " nsecs: 37099599\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8428,8 +8455,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 845581769\n", + " secs: 1706544922\n", + " nsecs: 67073106\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8444,8 +8471,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 865120410\n", + " secs: 1706544922\n", + " nsecs: 86748838\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8460,8 +8487,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 884716749\n", + " secs: 1706544922\n", + " nsecs: 105825424\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8476,8 +8503,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 894847631\n", + " secs: 1706544922\n", + " nsecs: 115547418\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8492,8 +8519,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 914685487\n", + " secs: 1706544922\n", + " nsecs: 134811162\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8508,8 +8535,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 924642562\n", + " secs: 1706544922\n", + " nsecs: 144512414\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8524,8 +8551,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 934620380\n", + " secs: 1706544922\n", + " nsecs: 154634714\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8540,8 +8567,8 @@ "CostmapLocation.Location(pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1704454730\n", - " nsecs: 944352626\n", + " secs: 1706544922\n", + " nsecs: 164654016\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -8556,28 +8583,6 @@ ] } ], - "source": [ - "from pycram.designators.object_designator import *\n", - "\n", - "milk_desig = BelieveObject(names=[\"Milk\"])\n", - "milk_desig.resolve()" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "## Location Designators\n", - "Location Designators can create a position in cartesian space from a symbolic description." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": { - "scrolled": false - }, - "outputs": [], "source": [ "from pycram.designators.location_designator import *\n", "from pycram.designators.object_designator import *\n", @@ -8604,11 +8609,11 @@ }, { "cell_type": "code", - "execution_count": 26, + "execution_count": 27, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:51.471552922Z", - "start_time": "2024-01-05T11:38:50.957762794Z" + "end_time": "2024-01-29T16:15:22.690059907Z", + "start_time": "2024-01-29T16:15:22.178420266Z" } }, "outputs": [], @@ -8630,11 +8635,11 @@ }, { "cell_type": "code", - "execution_count": 27, + "execution_count": 28, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:38:51.532465685Z", - "start_time": "2024-01-05T11:38:51.473570735Z" + "end_time": "2024-01-29T16:15:22.784167007Z", + "start_time": "2024-01-29T16:15:22.739698077Z" } }, "outputs": [], @@ -8645,11 +8650,11 @@ }, { "cell_type": "code", - "execution_count": 28, + "execution_count": 29, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.490830292Z", - "start_time": "2024-01-05T11:38:51.542100060Z" + "end_time": "2024-01-29T16:15:37.008203353Z", + "start_time": "2024-01-29T16:15:22.784090487Z" } }, "outputs": [], @@ -8695,11 +8700,11 @@ }, { "cell_type": "code", - "execution_count": 29, + "execution_count": 30, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.494121167Z", - "start_time": "2024-01-05T11:39:03.491538560Z" + "end_time": "2024-01-29T16:15:37.015721112Z", + "start_time": "2024-01-29T16:15:37.010004750Z" } }, "outputs": [ @@ -8708,26 +8713,26 @@ "output_type": "stream", "text": [ "no_operation()\n", - "├── perform(Motion)\n", - "├── perform(Motion)\n", + "├── perform(MoveMotion)\n", + "├── perform(MoveMotion)\n", "├── perform(ParkArmsActionPerformable)\n", "├── perform(ParkArmsActionPerformable)\n", "├── perform(MoveTorsoActionPerformable)\n", "├── perform(NavigateActionPerformable)\n", - "│ └── perform(Motion)\n", + "│ └── perform(MoveMotion)\n", "├── perform(PickUpActionPerformable)\n", - "│ ├── perform(Motion)\n", - "│ ├── perform(Motion)\n", - "│ ├── perform(Motion)\n", - "│ ├── perform(Motion)\n", - "│ └── perform(Motion)\n", + "│ ├── perform(MoveTCPMotion)\n", + "│ ├── perform(MoveGripperMotion)\n", + "│ ├── perform(MoveTCPMotion)\n", + "│ ├── perform(MoveGripperMotion)\n", + "│ └── perform(MoveTCPMotion)\n", "├── perform(ParkArmsActionPerformable)\n", "├── perform(NavigateActionPerformable)\n", - "│ └── perform(Motion)\n", + "│ └── perform(MoveMotion)\n", "├── perform(PlaceActionPerformable)\n", - "│ ├── perform(Motion)\n", - "│ ├── perform(Motion)\n", - "│ └── perform(Motion)\n", + "│ ├── perform(MoveTCPMotion)\n", + "│ ├── perform(MoveGripperMotion)\n", + "│ └── perform(MoveTCPMotion)\n", "└── perform(ParkArmsActionPerformable)\n" ] } @@ -8741,11 +8746,11 @@ }, { "cell_type": "code", - "execution_count": 30, + "execution_count": 31, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.669289460Z", - "start_time": "2024-01-05T11:39:03.494369670Z" + "end_time": "2024-01-29T16:15:37.200246997Z", + "start_time": "2024-01-29T16:15:37.012295886Z" } }, "outputs": [], @@ -8763,11 +8768,11 @@ }, { "cell_type": "code", - "execution_count": 31, + "execution_count": 32, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.922190909Z", - "start_time": "2024-01-05T11:39:03.675069105Z" + "end_time": "2024-01-29T16:15:37.447175714Z", + "start_time": "2024-01-29T16:15:37.236464380Z" } }, "outputs": [ @@ -8775,14 +8780,14 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 22/22 [00:00<00:00, 116.49it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 22/22 [00:00<00:00, 119.33it/s]\n" ] }, { "data": { - "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 38, 17, 681423), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" + "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 14, 50, 893086), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" }, - "execution_count": 31, + "execution_count": 32, "metadata": {}, "output_type": "execute_result" } @@ -8806,11 +8811,11 @@ }, { "cell_type": "code", - "execution_count": 32, + "execution_count": 33, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.966372243Z", - "start_time": "2024-01-05T11:39:03.922350989Z" + "end_time": "2024-01-29T16:15:37.488726709Z", + "start_time": "2024-01-29T16:15:37.444290355Z" } }, "outputs": [ @@ -8824,17 +8829,19 @@ } ], "source": [ - "navigations = session.query(pycram.orm.action_designator.NavigateAction).all()\n", + "from sqlalchemy import select\n", + "\n", + "navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all()\n", "print(*navigations, sep=\"\\n\")" ] }, { "cell_type": "code", - "execution_count": 33, + "execution_count": 34, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:39:03.967890749Z", - "start_time": "2024-01-05T11:39:03.966131716Z" + "end_time": "2024-01-29T16:15:37.489720308Z", + "start_time": "2024-01-29T16:15:37.488078620Z" } }, "outputs": [ @@ -8842,18 +8849,16 @@ "name": "stdout", "output_type": "stream", "text": [ - "(NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7), Position(id=7, x=0.5799999999999998, y=1.28, z=0.0, process_metadata_id=1), Quaternion(id=7, x=0.0, y=0.0, z=0.16378361324016077, w=-0.9864962889104031, process_metadata_id=1))\n", - "(NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16), Position(id=16, x=-1.9074999952316287, y=0.779200015068054, z=0.0, process_metadata_id=1), Quaternion(id=16, x=0.0, y=0.0, z=0.16439898730535735, w=0.9863939238321437, process_metadata_id=1))\n" + "NavigateAction(id=6, process_metadata_id=1, dtype='NavigateAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), pose_id=7)\n", + "NavigateAction(id=15, process_metadata_id=1, dtype='NavigateAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), pose_id=16)\n" ] } ], "source": [ - "navigations = (session.query(pycram.orm.action_designator.NavigateAction, \n", - " pycram.orm.base.Position, \n", - " pycram.orm.base.Quaternion).\n", - " join(pycram.orm.action_designator.NavigateAction.pose).\n", - " join(pycram.orm.base.Pose.position).\n", - " join(pycram.orm.base.Pose.orientation).all())\n", + "navigations = (session.scalars(select(pycram.orm.action_designator.NavigateAction, pycram.orm.base.Position, pycram.orm.base.Quaternion).\n", + " join(pycram.orm.action_designator.NavigateAction.pose).\n", + " join(pycram.orm.base.Pose.position).\n", + " join(pycram.orm.base.Pose.orientation)).all())\n", "print(*navigations, sep=\"\\n\")" ] }, @@ -8868,7 +8873,7 @@ }, { "cell_type": "code", - "execution_count": 34, + "execution_count": 35, "outputs": [], "source": [ "world.exit()" @@ -8876,8 +8881,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:39:04.199668442Z", - "start_time": "2024-01-05T11:39:03.966287501Z" + "end_time": "2024-01-29T16:15:37.732405944Z", + "start_time": "2024-01-29T16:15:37.488193658Z" } } } diff --git a/examples/local_transformer.ipynb b/examples/local_transformer.ipynb index 2d4d41c03..8a6352cde 100644 --- a/examples/local_transformer.ipynb +++ b/examples/local_transformer.ipynb @@ -17,14 +17,28 @@ "id": "615cb805", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:51:08.901104329Z", - "start_time": "2023-09-08T16:51:08.139879012Z" + "end_time": "2024-01-29T16:28:32.010116177Z", + "start_time": "2024-01-29T16:28:31.439191974Z" } }, - "outputs": [], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "pybullet build time: May 20 2022 19:44:17\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1706545711.997153]: Failed to import Giskard messages\n", + "[WARN] [1706545712.001630]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" + ] + } + ], "source": [ "from pycram.bullet_world import BulletWorld, Object\n", - "from pycram.pose import Transform\n", + "from pycram.pose import Transform, Pose\n", "from pycram.local_transformer import LocalTransformer" ] }, @@ -46,8 +60,8 @@ "id": "7a3222c2", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:51:11.202198685Z", - "start_time": "2023-09-08T16:51:10.916529902Z" + "end_time": "2024-01-29T16:28:32.323570586Z", + "start_time": "2024-01-29T16:28:32.085550201Z" } }, "outputs": [ @@ -55,8 +69,6 @@ "name": "stderr", "output_type": "stream", "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n" ] @@ -77,9 +89,14 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 3, "id": "c019c259", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:28:32.326842316Z", + "start_time": "2024-01-29T16:28:32.324154465Z" + } + }, "outputs": [], "source": [ "world.exit()" @@ -100,12 +117,12 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 4, "id": "14494539", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:51:38.439962987Z", - "start_time": "2023-09-08T16:51:33.675944073Z" + "end_time": "2024-01-29T16:28:35.474604282Z", + "start_time": "2024-01-29T16:28:32.329041645Z" } }, "outputs": [ @@ -113,6 +130,8 @@ "name": "stderr", "output_type": "stream", "text": [ + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Scalar element defined multiple times: limit\n", @@ -144,16 +163,21 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "id": "708606eb", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:28:35.481284612Z", + "start_time": "2024-01-29T16:28:35.472994214Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "\n", - "\n" + "\n", + "\n" ] } ], @@ -178,12 +202,12 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 6, "id": "988ffda1", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:42:15.478051994Z", - "start_time": "2023-09-08T16:42:15.472743693Z" + "end_time": "2024-01-29T16:28:35.509096003Z", + "start_time": "2024-01-29T16:28:35.483520847Z" } }, "outputs": [ @@ -194,8 +218,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448290\n", - " nsecs: 552922010\n", + " secs: 1706545715\n", + " nsecs: 495329141\n", " frame_id: \"milk_4\"\n", "pose: \n", " position: \n", @@ -210,8 +234,8 @@ "header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448290\n", - " nsecs: 553559541\n", + " secs: 1706545715\n", + " nsecs: 496921062\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -228,7 +252,6 @@ ], "source": [ "from pycram.local_transformer import LocalTransformer\n", - "from pycram.pose import Pose\n", "\n", "l = LocalTransformer()\n", "test_pose = Pose([1, 1, 1], [0, 0, 0, 1], \"map\")\n", @@ -251,9 +274,14 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 7, "id": "dd2bafc5", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:28:35.509520802Z", + "start_time": "2024-01-29T16:28:35.500190623Z" + } + }, "outputs": [], "source": [ "from pycram.pose import Transform\n", @@ -274,12 +302,12 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": 8, "id": "6b23f540", "metadata": { "ExecuteTime": { - "end_time": "2023-09-08T16:47:16.375950749Z", - "start_time": "2023-09-08T16:47:16.332420624Z" + "end_time": "2024-01-29T16:28:35.509787045Z", + "start_time": "2024-01-29T16:28:35.504069535Z" } }, "outputs": [], @@ -303,9 +331,14 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 9, "id": "bd619710", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:28:35.552242116Z", + "start_time": "2024-01-29T16:28:35.508694526Z" + } + }, "outputs": [ { "name": "stdout", diff --git a/examples/migrate_neems.ipynb b/examples/migrate_neems.ipynb index db3baabe5..8f8064c0d 100644 --- a/examples/migrate_neems.ipynb +++ b/examples/migrate_neems.ipynb @@ -128,7 +128,7 @@ " @with_tree\n", " def pick_and_place_plan(self):\n", " with simulated_robot:\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n", + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n", " MoveTorsoAction([0.3]).resolve().perform()\n", " pickup_pose = CostmapLocation(target=self.cereal_desig.resolve(), reachable_for=self.robot_desig).resolve()\n", " pickup_arm = pickup_pose.reachable_arms[0]\n", @@ -147,7 +147,7 @@ "\n", " PlaceAction(self.cereal_desig, target_locations=[place_island.pose], arms=[pickup_arm]).resolve().perform()\n", "\n", - " ParkArmsAction.Action(Arms.BOTH).perform()\n" + " ParkArmsAction([Arms.BOTH]).resolve().perform()\n" ] }, { diff --git a/examples/motion_designator.ipynb b/examples/motion_designator.ipynb index 9b0dbb8b6..2a0b256de 100644 --- a/examples/motion_designator.ipynb +++ b/examples/motion_designator.ipynb @@ -15,23 +15,35 @@ "cell_type": "code", "execution_count": 1, "id": "7f7c74ae", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.051126267Z", + "start_time": "2024-01-29T16:46:56.312023994Z" + } + }, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ - "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", - "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", + "pybullet build time: May 20 2022 19:44:17\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1706546816.907019]: Failed to import Giskard messages\n", + "[WARN] [1706546816.911886]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", + "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", + "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n" ] } @@ -54,9 +66,14 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 2, "id": "cc2e1bcd", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.051774984Z", + "start_time": "2024-01-29T16:47:00.048239557Z" + } + }, "outputs": [], "source": [ "world.exit()" @@ -73,27 +90,36 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": 3, "id": "ad9c75e3", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.576314628Z", + "start_time": "2024-01-29T16:47:00.051456576Z" + } + }, "outputs": [], "source": [ + "from pycram.pose import Pose\n", "from pycram.designators.motion_designator import MoveMotion\n", "from pycram.process_module import simulated_robot\n", "\n", "with simulated_robot:\n", " motion_description = MoveMotion(target=Pose([1, 0, 0], [0, 0, 0, 1]))\n", " \n", - " motion_desig = motion_description.resolve()\n", - " \n", - " motion_desig.perform()" + " motion_description.perform()" ] }, { "cell_type": "code", "execution_count": 4, "id": "ac9f7766", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:00.594527954Z", + "start_time": "2024-01-29T16:47:00.591545638Z" + } + }, "outputs": [], "source": [ "world.reset_bullet_world()" @@ -112,7 +138,12 @@ "cell_type": "code", "execution_count": 5, "id": "52aa961d", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:01.122650746Z", + "start_time": "2024-01-29T16:47:00.593841452Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import MoveTCPMotion\n", @@ -121,7 +152,7 @@ "with simulated_robot:\n", " motion_description = MoveTCPMotion(target=Pose([0.5, 0.6, 0.6], [0, 0, 0, 1]), arm=\"left\")\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -137,7 +168,12 @@ "cell_type": "code", "execution_count": 6, "id": "84ffcf03", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:01.633288773Z", + "start_time": "2024-01-29T16:47:01.135133088Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import LookingMotion\n", @@ -146,7 +182,7 @@ "with simulated_robot:\n", " motion_description = LookingMotion(target=Pose([1, 1, 1], [0, 0, 0, 1]))\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -162,7 +198,12 @@ "cell_type": "code", "execution_count": 7, "id": "f90adb34", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:02.149528442Z", + "start_time": "2024-01-29T16:47:01.647168572Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import MoveGripperMotion\n", @@ -171,7 +212,7 @@ "with simulated_robot:\n", " motion_description = MoveGripperMotion(motion=\"open\", gripper=\"left\")\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -189,7 +230,12 @@ "cell_type": "code", "execution_count": 8, "id": "4c9f3e27", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:02.199992775Z", + "start_time": "2024-01-29T16:47:02.150827945Z" + } + }, "outputs": [], "source": [ "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.5, 0, 1]))" @@ -197,21 +243,26 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 9, "id": "27eae08a", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:03.473751479Z", + "start_time": "2024-01-29T16:47:02.201339779Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "ObjectDesignatorDescription.Object(name=milk, type=ObjectType.MILK, bullet_world_object=Object(world=, \n", - "local_transformer=, \n", + "ObjectDesignatorDescription.Object(name=milk, type=ObjectType.MILK, bullet_world_object=Object(world=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -219,8 +270,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -237,8 +288,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -250,48 +301,17 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", + "_current_link_poses: ..., \n", + "_current_link_transforms: ..., \n", "_current_joint_states={}, \n", "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': }), _pose=, \n", - "local_transformer=, \n", + "link_to_geometry: ...), _pose=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -299,8 +319,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -317,8 +337,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -330,46 +350,15 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", + "_current_link_poses: ..., \n", + "_current_link_transforms: ..., \n", "_current_joint_states={}, \n", "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': })>, pose=header: \n", + "link_to_geometry: ...)>, pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -389,11 +378,11 @@ "from pycram.process_module import simulated_robot\n", "\n", "with simulated_robot:\n", - " LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).resolve().perform()\n", + " LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform()\n", " \n", " motion_description = DetectingMotion(object_type=ObjectType.MILK)\n", " \n", - " obj = motion_description.resolve().perform()\n", + " obj = motion_description.perform()\n", " \n", " print(obj)" ] @@ -404,23 +393,28 @@ "metadata": {}, "source": [ "## Move Arm Joints\n", - "This motion designator moves one or both arms. Movement targets can either be a dictionary with joint name as key and target pose as value or a pre-defined configuration like 'park'." + "This motion designator moves one or both arms. Movement targets are a dictionary with joint name as key and target pose as value. " ] }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 10, "id": "ce7655c1", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:03.979196754Z", + "start_time": "2024-01-29T16:47:03.482859314Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import MoveArmJointsMotion\n", "from pycram.process_module import simulated_robot\n", "\n", "with simulated_robot:\n", - " motion_description = MoveArmJointsMotion(left_arm_config=\"park\", right_arm_poses={\"r_shoulder_pan_joint\": -0.7})\n", + " motion_description = MoveArmJointsMotion(right_arm_poses={\"r_shoulder_pan_joint\": -0.7})\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] }, { @@ -436,9 +430,14 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 11, "id": "0f9f0ca8", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:04.035948673Z", + "start_time": "2024-01-29T16:47:03.980202995Z" + } + }, "outputs": [], "source": [ "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([-1, 0, 1]))" @@ -446,21 +445,26 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 12, "id": "4382bfa3", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:04.540918323Z", + "start_time": "2024-01-29T16:47:04.035986657Z" + } + }, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Object(world=, \n", - "local_transformer=, \n", + "Object(world=, \n", + "local_transformer=, \n", "name=milk, \n", "type=ObjectType.MILK, \n", "color=[1, 1, 1, 1], \n", "id=3, \n", - "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n", + "path=/home/dprueser/workspace/ros/src/pycram/src/pycram/../../resources/cached/milk.urdf, \n", "joints: ..., \n", "links: ..., \n", "attachments: ..., \n", @@ -468,8 +472,8 @@ "original_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -486,8 +490,8 @@ "_current_pose=header: \n", " seq: 0\n", " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", + " secs: 1706546822\n", + " nsecs: 149801731\n", " frame_id: \"map\"\n", "pose: \n", " position: \n", @@ -499,42 +503,11 @@ " y: 0.0\n", " z: 0.0\n", " w: 1.0, \n", - "_current_link_poses={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448584\n", - " nsecs: 433914899\n", - " frame_id: \"map\"\n", - "pose: \n", - " position: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " orientation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", - "_current_link_transforms={'milk_main': header: \n", - " seq: 0\n", - " stamp: \n", - " secs: 1699448598\n", - " nsecs: 510938167\n", - " frame_id: \"map\"\n", - "child_frame_id: \"milk_3\"\n", - "transform: \n", - " translation: \n", - " x: 1.5\n", - " y: 0.0\n", - " z: 1.0\n", - " rotation: \n", - " x: 0.0\n", - " y: 0.0\n", - " z: 0.0\n", - " w: 1.0}, \n", + "_current_link_poses: ..., \n", + "_current_link_transforms: ..., \n", "_current_joint_states={}, \n", "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05 8.96554102e-02], \n", - "link_to_geometry={'milk_main': })\n" + "link_to_geometry: ...)\n" ] } ], @@ -545,7 +518,7 @@ "with simulated_robot:\n", " motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK)\n", " \n", - " obj = motion_description.resolve().perform()\n", + " obj = motion_description.perform()\n", " \n", " print(obj)" ] @@ -561,9 +534,14 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 13, "id": "1e0191c8", - "metadata": {}, + "metadata": { + "ExecuteTime": { + "end_time": "2024-01-29T16:47:05.046180891Z", + "start_time": "2024-01-29T16:47:04.548113629Z" + } + }, "outputs": [], "source": [ "from pycram.designators.motion_designator import MoveJointsMotion\n", @@ -572,7 +550,7 @@ "with simulated_robot:\n", " motion_description = MoveJointsMotion(names=[\"torso_lift_joint\", \"r_shoulder_pan_joint\"], positions=[0.2, -1.2])\n", " \n", - " motion_description.resolve().perform()" + " motion_description.perform()" ] } ], diff --git a/examples/orm_example.ipynb b/examples/orm_example.ipynb index dc0b3e711..26e810c9c 100644 --- a/examples/orm_example.ipynb +++ b/examples/orm_example.ipynb @@ -15,14 +15,14 @@ "execution_count": 1, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:06.779230216Z", - "start_time": "2024-01-05T11:48:06.588261981Z" + "end_time": "2024-01-29T16:51:09.128875369Z", + "start_time": "2024-01-29T16:51:08.956260897Z" } }, "outputs": [ { "data": { - "text/plain": "" + "text/plain": "" }, "execution_count": 1, "metadata": {}, @@ -50,8 +50,8 @@ "execution_count": 2, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:07.264463765Z", - "start_time": "2024-01-05T11:48:06.770051795Z" + "end_time": "2024-01-29T16:51:09.568683265Z", + "start_time": "2024-01-29T16:51:09.124071834Z" } }, "outputs": [ @@ -64,8 +64,8 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1704455287.243634]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1704455287.248237]: Failed to import Giskard messages\n" + "[WARN] [1706547069.541495]: Failed to import Giskard messages\n", + "[WARN] [1706547069.546682]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], @@ -88,8 +88,8 @@ "execution_count": 3, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.697221588Z", - "start_time": "2024-01-05T11:48:07.267874656Z" + "end_time": "2024-01-29T16:51:35.305706324Z", + "start_time": "2024-01-29T16:51:09.568597034Z" } }, "outputs": [ @@ -105,14 +105,62 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n", "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n", - "Scalar element defined multiple times: limit\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "Scalar element defined multiple times: limit\n", "Scalar element defined multiple times: limit\n" ] }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "argc=2\n", + "argv[0] = --unused\n", + "argv[1] = --start_demo_name=Physics Server\n", + "ExampleBrowserThreadFunc started\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "X11 functions dynamically loaded using dlopen/dlsym OK!\n", + "Creating context\n", + "Created GL 3.3 context\n", + "Direct GLX rendering context obtained\n", + "Making context current\n", + "GL_VENDOR=AMD\n", + "GL_RENDERER=AMD Radeon RX 6700 XT (NAVY_FLOUNDER, DRM 3.42.0, 5.15.0-89-generic, LLVM 12.0.0)\n", + "GL_VERSION=4.6 (Core Profile) Mesa 21.2.6\n", + "GL_SHADING_LANGUAGE_VERSION=4.60\n", + "pthread_getconcurrency()=0\n", + "Version = 4.6 (Core Profile) Mesa 21.2.6\n", + "Vendor = AMD\n", + "Renderer = AMD Radeon RX 6700 XT (NAVY_FLOUNDER, DRM 3.42.0, 5.15.0-89-generic, LLVM 12.0.0)\n", + "b3Printf: Selected demo: Physics Server\n", + "startThreads creating 1 threads.\n", + "starting thread 0\n", + "started thread 0 \n", + "MotionThreadFunc thread started\n", + "ven = AMD\n", + "ven = AMD\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/r_gripper_r_finger_tip_link (parent map) at time 1706547087.197074 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/r_gripper_palm_link (parent map) at time 1706547087.480158 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/projector_wg6802418_child_frame (parent map) at time 1706547089.689294 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame shadow/pr2_1/l_shoulder_pan_link (parent map) at time 1706547092.019791 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n" + ] + }, { "name": "stdout", "output_type": "stream", @@ -122,20 +170,20 @@ " ├── perform(ParkArmsActionPerformable)\n", " ├── perform(MoveTorsoActionPerformable)\n", " ├── perform(NavigateActionPerformable)\n", - " │ └── perform(Motion)\n", + " │ └── perform(MoveMotion)\n", " ├── perform(PickUpActionPerformable)\n", - " │ ├── perform(Motion)\n", - " │ ├── perform(Motion)\n", - " │ ├── perform(Motion)\n", - " │ ├── perform(Motion)\n", - " │ └── perform(Motion)\n", + " │ ├── perform(MoveTCPMotion)\n", + " │ ├── perform(MoveGripperMotion)\n", + " │ ├── perform(MoveTCPMotion)\n", + " │ ├── perform(MoveGripperMotion)\n", + " │ └── perform(MoveTCPMotion)\n", " ├── perform(ParkArmsActionPerformable)\n", " ├── perform(NavigateActionPerformable)\n", - " │ └── perform(Motion)\n", + " │ └── perform(MoveMotion)\n", " ├── perform(PlaceActionPerformable)\n", - " │ ├── perform(Motion)\n", - " │ ├── perform(Motion)\n", - " │ └── perform(Motion)\n", + " │ ├── perform(MoveTCPMotion)\n", + " │ ├── perform(MoveGripperMotion)\n", + " │ └── perform(MoveTCPMotion)\n", " └── perform(ParkArmsActionPerformable)\n" ] } @@ -204,8 +252,8 @@ "execution_count": 4, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.882423457Z", - "start_time": "2024-01-05T11:48:27.695892994Z" + "end_time": "2024-01-29T16:51:35.505074180Z", + "start_time": "2024-01-29T16:51:35.307461245Z" } }, "outputs": [ @@ -213,12 +261,12 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 112.65it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 20/20 [00:00<00:00, 105.04it/s]\n" ] }, { "data": { - "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" + "text/plain": "TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1)" }, "execution_count": 4, "metadata": {}, @@ -241,8 +289,8 @@ "execution_count": 5, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.887896771Z", - "start_time": "2024-01-05T11:48:27.883057209Z" + "end_time": "2024-01-29T16:51:35.508649071Z", + "start_time": "2024-01-29T16:51:35.506023628Z" } }, "outputs": [ @@ -250,12 +298,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "ProcessMetaData(id=1, created_at=datetime.datetime(2024, 1, 5, 11, 48, 27), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='be748996363cb497af6784f6f2aaeffcb2fb1c42')\n" + "ProcessMetaData(id=1, created_at=datetime.datetime(2024, 1, 29, 16, 51, 35), created_by='dprueser', description='Tutorial for getting familiar with the ORM.', pycram_version='5e32afdb9ba57a9d6befec2c78087fee1f5e5d9b')\n" ] } ], "source": [ - "print(*session.query(pycram.orm.base.ProcessMetaData).all())" + "from sqlalchemy import select\n", + "\n", + "print(*session.scalars(select(pycram.orm.base.ProcessMetaData)).all())" ] }, { @@ -271,8 +321,8 @@ "execution_count": 6, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.933255911Z", - "start_time": "2024-01-05T11:48:27.886082312Z" + "end_time": "2024-01-29T16:51:35.520518056Z", + "start_time": "2024-01-29T16:51:35.508477111Z" } }, "outputs": [ @@ -286,7 +336,7 @@ } ], "source": [ - "navigations = session.query(pycram.orm.action_designator.NavigateAction).all()\n", + "navigations = session.scalars(select(pycram.orm.action_designator.NavigateAction)).all()\n", "print(*navigations, sep=\"\\n\")" ] }, @@ -302,8 +352,8 @@ "execution_count": 7, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.933794333Z", - "start_time": "2024-01-05T11:48:27.930158041Z" + "end_time": "2024-01-29T16:51:35.560192686Z", + "start_time": "2024-01-29T16:51:35.520628950Z" } }, "outputs": [ @@ -323,7 +373,7 @@ } ], "source": [ - "actions = session.query(pycram.orm.action_designator.Action).all()\n", + "actions = session.scalars(select(pycram.orm.action_designator.Action)).all()\n", "print(*actions, sep=\"\\n\")" ] }, @@ -339,8 +389,8 @@ "execution_count": 8, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.934174898Z", - "start_time": "2024-01-05T11:48:27.930328964Z" + "end_time": "2024-01-29T16:51:35.561234983Z", + "start_time": "2024-01-29T16:51:35.535162323Z" } }, "outputs": [ @@ -348,14 +398,14 @@ "name": "stdout", "output_type": "stream", "text": [ - "Pose(id=7, time=datetime.datetime(2024, 1, 5, 11, 48, 13, 104805), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" + "Pose(id=7, time=datetime.datetime(2024, 1, 29, 16, 51, 15, 804838), frame='map', position_id=7, orientation_id=7, process_metadata_id=1)\n" ] } ], "source": [ - "object_actions = (session.query(pycram.orm.base.Pose)\n", + "object_actions = (session.scalars(select(pycram.orm.base.Pose)\n", " .join(pycram.orm.action_designator.PickUpAction.object)\n", - " .join(pycram.orm.object_designator.Object.pose)\n", + " .join(pycram.orm.object_designator.Object.pose))\n", " .all())\n", "print(*object_actions, sep=\"\\n\")\n" ] @@ -384,8 +434,8 @@ "execution_count": 9, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.992321580Z", - "start_time": "2024-01-05T11:48:27.930434353Z" + "end_time": "2024-01-29T16:51:35.600701170Z", + "start_time": "2024-01-29T16:51:35.539887927Z" } }, "outputs": [ @@ -393,30 +443,30 @@ "name": "stdout", "output_type": "stream", "text": [ - "TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167962), end_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 674674), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=4, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 674769), end_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 177846), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742815), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288620), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=6, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742855), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288611), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 14, 742815), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288620), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=8, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 417504), end_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 929845), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 929939), end_time=datetime.datetime(2024, 1, 5, 12, 48, 16, 435586), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 16, 568469), end_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 76882), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 77011), end_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 589849), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=12, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 17, 720082), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 228417), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 15, 288722), end_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 519955), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=13, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 18, 520072), end_time=datetime.datetime(2024, 1, 5, 12, 48, 19, 27331), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115462), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625686), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115512), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625678), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 115462), end_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625686), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 626977), end_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 135284), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=18, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 135369), end_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 642910), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=19, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 26, 643067), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153860), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 25, 625795), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153873), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", - "TaskTreeNode(id=20, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 153953), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660007), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 13, 167898), end_time=datetime.datetime(2024, 1, 5, 12, 48, 27, 660020), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 5, 12, 48, 7, 241508), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" + "TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=3, code_id=3, code=Code(id=3, function='perform', designator_id=1, designator=ParkArmsAction(id=1, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=1, robot_state=RobotState(id=1, torso_height=0.0, type=, pose_id=1, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872243), end_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 378193), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=4, code_id=4, code=Code(id=4, function='perform', designator_id=2, designator=MoveTorsoAction(id=2, process_metadata_id=1, dtype='MoveTorsoAction', robot_state_id=2, robot_state=RobotState(id=2, torso_height=0.0, type=, pose_id=2, process_metadata_id=1), position=0.3), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 378288), end_time=datetime.datetime(2024, 1, 29, 17, 51, 16, 881639), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459617), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970219), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=6, code_id=6, code=Code(id=6, function='perform', designator_id=4, designator=MoveMotion(id=4, process_metadata_id=1, dtype='MoveMotion', pose_id=5), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459689), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970208), status=, reason=None, parent_id=5, parent=TaskTreeNode(id=5, code_id=5, code=Code(id=5, function='perform', designator_id=3, designator=NavigateAction(id=3, process_metadata_id=1, dtype='NavigateAction', robot_state_id=3, robot_state=RobotState(id=3, torso_height=0.3, type=, pose_id=3, process_metadata_id=1), pose_id=4), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 459617), end_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970219), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=8, code_id=8, code=Code(id=8, function='perform', designator_id=6, designator=MoveTCPMotion(id=6, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=8), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 122271), end_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 631460), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=9, code_id=9, code=Code(id=9, function='perform', designator_id=7, designator=MoveGripperMotion(id=7, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 18, 631582), end_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 141918), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=10, code_id=10, code=Code(id=10, function='perform', designator_id=8, designator=MoveTCPMotion(id=8, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=9), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 273490), end_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 783950), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=11, code_id=11, code=Code(id=11, function='perform', designator_id=9, designator=MoveGripperMotion(id=9, process_metadata_id=1, dtype='MoveGripperMotion', motion='close', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 19, 784120), end_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 298210), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=12, code_id=12, code=Code(id=12, function='perform', designator_id=10, designator=MoveTCPMotion(id=10, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=10), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 457629), end_time=datetime.datetime(2024, 1, 29, 17, 51, 20, 974496), status=, reason=None, parent_id=7, parent=TaskTreeNode(id=7, code_id=7, code=Code(id=7, function='perform', designator_id=5, designator=PickUpAction(id=5, process_metadata_id=1, dtype='PickUpAction', robot_state_id=4, robot_state=RobotState(id=4, torso_height=0.3, type=, pose_id=6, process_metadata_id=1), arm='left', grasp='front', object_id=1), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 17, 970355), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274534), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=13, code_id=13, code=Code(id=13, function='perform', designator_id=11, designator=ParkArmsAction(id=11, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=5, robot_state=RobotState(id=5, torso_height=0.3, type=, pose_id=11, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 274678), end_time=datetime.datetime(2024, 1, 29, 17, 51, 21, 782383), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 702999), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209586), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=15, code_id=15, code=Code(id=15, function='perform', designator_id=13, designator=MoveMotion(id=13, process_metadata_id=1, dtype='MoveMotion', pose_id=14), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 703060), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209577), status=, reason=None, parent_id=14, parent=TaskTreeNode(id=14, code_id=14, code=Code(id=14, function='perform', designator_id=12, designator=NavigateAction(id=12, process_metadata_id=1, dtype='NavigateAction', robot_state_id=6, robot_state=RobotState(id=6, torso_height=0.3, type=, pose_id=12, process_metadata_id=1), pose_id=13), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 32, 702999), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209586), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=17, code_id=17, code=Code(id=17, function='perform', designator_id=15, designator=MoveTCPMotion(id=15, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=18), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 210786), end_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 721856), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=18, code_id=18, code=Code(id=18, function='perform', designator_id=16, designator=MoveGripperMotion(id=16, process_metadata_id=1, dtype='MoveGripperMotion', motion='open', gripper='left', allow_gripper_collision=None), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 722026), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 239587), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=19, code_id=19, code=Code(id=19, function='perform', designator_id=17, designator=MoveTCPMotion(id=17, process_metadata_id=1, dtype='MoveTCPMotion', arm='left', allow_gripper_collision=None, pose_id=19), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 241042), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751946), status=, reason=None, parent_id=16, parent=TaskTreeNode(id=16, code_id=16, code=Code(id=16, function='perform', designator_id=14, designator=PlaceAction(id=14, process_metadata_id=1, dtype='PlaceAction', robot_state_id=7, robot_state=RobotState(id=7, torso_height=0.3, type=, pose_id=15, process_metadata_id=1), arm='left', pose_id=17, object_id=2), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 33, 209685), end_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 751975), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n", + "TaskTreeNode(id=20, code_id=20, code=Code(id=20, function='perform', designator_id=18, designator=ParkArmsAction(id=18, process_metadata_id=1, dtype='ParkArmsAction', robot_state_id=8, robot_state=RobotState(id=8, torso_height=0.3, type=, pose_id=20, process_metadata_id=1), arm=), process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 34, 752094), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265524), status=, reason=None, parent_id=2, parent=TaskTreeNode(id=2, code_id=2, code=Code(id=2, function='plan', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 15, 872198), end_time=datetime.datetime(2024, 1, 29, 17, 51, 35, 265539), status=, reason=None, parent_id=1, parent=TaskTreeNode(id=1, code_id=1, code=Code(id=1, function='no_operation', designator_id=None, designator=None, process_metadata_id=1), start_time=datetime.datetime(2024, 1, 29, 17, 51, 9, 545708), end_time=None, status=, reason=None, parent_id=None, parent=None, process_metadata_id=1), process_metadata_id=1), process_metadata_id=1)\n" ] } ], "source": [ - "successful_tasks = session.query(pycram.orm.task.TaskTreeNode).filter(pycram.orm.task.TaskTreeNode.status == \"SUCCEEDED\")\n", + "successful_tasks = session.scalars(select(pycram.orm.task.TaskTreeNode).where(pycram.orm.task.TaskTreeNode.status == \"SUCCEEDED\"))\n", "print(*successful_tasks, sep=\"\\n\")" ] }, @@ -434,13 +484,13 @@ "execution_count": 10, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.993029221Z", - "start_time": "2024-01-05T11:48:27.956265531Z" + "end_time": "2024-01-29T16:51:35.601121928Z", + "start_time": "2024-01-29T16:51:35.600085428Z" } }, "outputs": [], "source": [ - "from sqlalchemy.orm import Mapped, mapped_column\n", + "from sqlalchemy.orm import Mapped, mapped_column, Session\n", "from pycram.orm.action_designator import Action\n", "from dataclasses import dataclass\n", "\n", @@ -485,8 +535,8 @@ "execution_count": 11, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:27.993997340Z", - "start_time": "2024-01-05T11:48:27.956444747Z" + "end_time": "2024-01-29T16:51:35.601417935Z", + "start_time": "2024-01-29T16:51:35.600240790Z" } }, "outputs": [], @@ -506,8 +556,8 @@ "execution_count": 12, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:28.491240632Z", - "start_time": "2024-01-05T11:48:27.965003339Z" + "end_time": "2024-01-29T16:51:36.020873055Z", + "start_time": "2024-01-29T16:51:35.600296911Z" } }, "outputs": [ @@ -522,7 +572,7 @@ "name": "stderr", "output_type": "stream", "text": [ - "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 77.00it/s]\n" + "Inserting TaskTree into database: 100%|██████████| 21/21 [00:00<00:00, 116.25it/s]\n" ] } ], @@ -545,11 +595,11 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 14, "metadata": { "ExecuteTime": { - "end_time": "2024-01-05T11:48:28.526329436Z", - "start_time": "2024-01-05T11:48:28.478332427Z" + "end_time": "2024-01-29T16:52:32.021378099Z", + "start_time": "2024-01-29T16:52:31.976674160Z" } }, "outputs": [ @@ -557,14 +607,22 @@ "data": { "text/plain": "[ORMSaying(id=37, process_metadata_id=1, dtype='ORMSaying', robot_state_id=17, robot_state=RobotState(id=17, torso_height=0.3, type=, pose_id=41, process_metadata_id=1), text='Patchie, Patchie; Where is my Patchie?')]" }, - "execution_count": 13, + "execution_count": 14, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "session.query(ORMSaying).all()" + "session.scalars(select(ORMSaying)).all()" ] + }, + { + "cell_type": "code", + "outputs": [], + "source": [], + "metadata": { + "collapsed": false + } } ], "metadata": { diff --git a/examples/orm_querying_examples.ipynb b/examples/orm_querying_examples.ipynb index 64f78edb4..12adedfe1 100644 --- a/examples/orm_querying_examples.ipynb +++ b/examples/orm_querying_examples.ipynb @@ -36,8 +36,8 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - "[WARN] [1704455774.294953]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n", - "[WARN] [1704455774.300327]: Failed to import Giskard messages\n" + "[WARN] [1706547279.009069]: Failed to import Giskard messages\n", + "[WARN] [1706547279.013875]: Could not import RoboKudo messages, RoboKudo interface could not be initialized\n" ] } ], @@ -206,8 +206,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:56:14.747051902Z", - "start_time": "2024-01-05T11:56:13.637965477Z" + "end_time": "2024-01-29T16:54:39.452156158Z", + "start_time": "2024-01-29T16:54:38.500054294Z" } }, "id": "dc0c9e6f15f126a3" @@ -243,9 +243,23 @@ "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", - " 74%|███████▍ | 708/960 [01:31<00:29, 8.48it/s, success_rate=0.0819]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_gripper_palm_link (parent map) at time 1704455865.925491 according to authority default_authority\n", + " 7%|▋ | 69/960 [00:10<01:47, 8.33it/s, success_rate=0.058] Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame floor_0 (parent map) at time 1706547289.526924 according to authority default_authority\n", " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", - "100%|██████████| 960/960 [02:03<00:00, 7.85it/s, success_rate=0.0854]" + " 28%|██▊ | 269/960 [00:38<01:37, 7.08it/s, success_rate=0.0929]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_frame (parent map) at time 1706547317.808086 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 33%|███▎ | 314/960 [00:44<01:35, 6.79it/s, success_rate=0.0955]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame floor_0 (parent map) at time 1706547323.909252 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 52%|█████▏ | 502/960 [01:10<01:07, 6.76it/s, success_rate=0.0837]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_frame (parent map) at time 1706547349.562933 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 57%|█████▋ | 543/960 [01:16<01:02, 6.65it/s, success_rate=0.081] Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/narrow_stereo_l_stereo_camera_optical_frame (parent map) at time 1706547355.536726 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 78%|███████▊ | 749/960 [01:44<00:28, 7.28it/s, success_rate=0.0814]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/torso_lift_motor_screw_link (parent map) at time 1706547384.042323 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 81%|████████▏ | 782/960 [01:48<00:22, 7.94it/s, success_rate=0.0831]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/l_torso_lift_side_plate_link (parent map) at time 1706547388.456061 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + " 86%|████████▌ | 826/960 [01:55<00:18, 7.12it/s, success_rate=0.0835]Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame pr2_1/r_shoulder_lift_link (parent map) at time 1706547394.924536 according to authority default_authority\n", + " at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.7/src/buffer_core.cpp\n", + "100%|██████████| 960/960 [02:13<00:00, 7.74it/s, success_rate=0.0854]" ] } ], @@ -261,8 +275,8 @@ "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:58:18.210175597Z", - "start_time": "2024-01-05T11:56:14.749214037Z" + "end_time": "2024-01-29T16:56:53.360247565Z", + "start_time": "2024-01-29T16:54:39.452769369Z" } }, "id": "f2dcacd942218899" @@ -296,9 +310,9 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"PickUpAction\".arm AS \"PickUpAction_arm\", \"PickUpAction\".grasp AS \"PickUpAction_grasp\", \"RobotState\".torso_height AS \"RobotState_torso_height\", \"Position\".x AS \"Position_x\", \"Position\".y AS \"Position_y\" \n", + "SELECT \"PickUpAction\".arm, \"PickUpAction\".grasp, \"RobotState\".torso_height, \"Position\".x, \"Position\".y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" ON \"Pose\".id = \"RobotState\".pose_id JOIN \"Position\" ON \"Position\".id = \"Pose\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id \n", - "WHERE \"Object\".type = ? AND \"TaskTreeNode\".status = ?\n", + "WHERE \"Object\".type = :type_1 AND \"TaskTreeNode\".status = :status_1\n", " arm grasp torso_height x y\n", "0 left left 0.198541 -0.679778 0.049746\n", "1 left left 0.092868 0.473199 0.612112\n", @@ -324,30 +338,31 @@ } ], "source": [ + "from sqlalchemy import select\n", "from pycram.enums import ObjectType\n", "\n", "milk = BulletWorldObject(\"Milk\", ObjectType.MILK, \"milk.stl\")\n", "\n", "# query all relative robot positions in regard to an objects position\n", "# make sure to order the joins() correctly\n", - "query = (session.query(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y)\n", + "query = (select(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y)\n", " .join(TaskTreeNode.code)\n", " .join(Code.designator.of_type(ORMPickUpAction))\n", " .join(ORMPickUpAction.robot_state)\n", " .join(RobotState.pose)\n", " .join(pycram.orm.base.Pose.position)\n", - " .join(ORMPickUpAction.object).filter(Object.type == milk.type)\n", - " .filter(TaskTreeNode.status == \"SUCCEEDED\"))\n", + " .join(ORMPickUpAction.object).where(Object.type == milk.type)\n", + " .where(TaskTreeNode.status == \"SUCCEEDED\"))\n", "print(query)\n", "\n", - "df = pd.read_sql_query(query.statement, session.get_bind())\n", + "df = pd.read_sql_query(query, session.get_bind())\n", "print(df)" ], "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:58:18.210316772Z", - "start_time": "2024-01-05T11:58:18.210052650Z" + "end_time": "2024-01-29T16:56:53.362279642Z", + "start_time": "2024-01-29T16:56:53.360090447Z" } }, "id": "b17c6366f317dd7" @@ -384,7 +399,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "SELECT \"TaskTreeNode\".status AS \"TaskTreeNode_status\", \"Object\".type AS \"Object_type\", \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", + "SELECT \"TaskTreeNode\".status, \"Object\".type, \"Position_1\".z - \"RobotState\".torso_height AS \"relative torso height\", \"Position_2\".x - \"Position_1\".x AS x, \"Position_2\".y - \"Position_1\".y AS y \n", "FROM \"TaskTreeNode\" JOIN \"Code\" ON \"Code\".id = \"TaskTreeNode\".code_id JOIN (\"Designator\" JOIN \"Action\" ON \"Designator\".id = \"Action\".id JOIN \"PickUpAction\" ON \"Action\".id = \"PickUpAction\".id) ON \"Designator\".id = \"Code\".designator_id JOIN \"RobotState\" ON \"RobotState\".id = \"Action\".robot_state_id JOIN \"Pose\" AS \"Pose_1\" ON \"Pose_1\".id = \"RobotState\".pose_id JOIN \"Position\" AS \"Position_2\" ON \"Position_2\".id = \"Pose_1\".position_id JOIN \"Object\" ON \"Object\".id = \"PickUpAction\".object_id JOIN \"Pose\" AS \"Pose_2\" ON \"Pose_2\".id = \"Object\".pose_id JOIN \"Position\" AS \"Position_1\" ON \"Position_1\".id = \"Pose_2\".position_id\n", " status type relative torso height x \\\n", "0 FAILED ObjectType.BREAKFAST_CEREAL 0.480005 -0.737416 \n", @@ -424,7 +439,7 @@ "robot_position = sqlalchemy.orm.aliased(Position)\n", "object_position = sqlalchemy.orm.aliased(Position)\n", "\n", - "query = (session.query(TaskTreeNode.status, Object.type, \n", + "query = (select(TaskTreeNode.status, Object.type, \n", " sqlalchemy.label(\"relative torso height\", object_position.z - RobotState.torso_height),\n", " sqlalchemy.label(\"x\", robot_position.x - object_position.x),\n", " sqlalchemy.label(\"y\", robot_position.y - object_position.y))\n", @@ -438,15 +453,15 @@ " .join(object_position, object_pose.position))\n", "print(query)\n", "\n", - "df = pd.read_sql(query.statement, session.get_bind())\n", + "df = pd.read_sql(query, session.get_bind())\n", "df[\"status\"] = df[\"status\"].apply(lambda x: str(x.name))\n", "print(df)" ], "metadata": { "collapsed": false, "ExecuteTime": { - "end_time": "2024-01-05T11:58:18.254184939Z", - "start_time": "2024-01-05T11:58:18.210187006Z" + "end_time": "2024-01-29T16:56:53.408414208Z", + "start_time": "2024-01-29T16:56:53.408012819Z" } }, "id": "a89d1a0f2a933475" From b03c6e2559b4fda726c0f5cfd3a39ddf77c82340 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?David=20Pr=C3=BCser?= Date: Thu, 1 Feb 2024 16:00:43 +0100 Subject: [PATCH 12/12] [orm] fixed sqlalchemy deprecation warnings --- src/pycram/orm/base.py | 21 +++++++++++---------- src/pycram/orm/object_designator.py | 7 ++++--- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index cc6356d5e..6bf7f55a9 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -2,6 +2,7 @@ import datetime import getpass import os +from dataclasses import field from typing import Optional import git @@ -27,7 +28,7 @@ def get_pycram_version_from_git() -> Optional[str]: return repo.head.object.hexsha -class _Base(DeclarativeBase): +class _Base(DeclarativeBase, MappedAsDataclass): """Dummy class""" type_annotation_map = { str: String(255) @@ -41,7 +42,7 @@ def __tablename__(self): return self.__name__ -class Base(_Base, MappedAsDataclass): +class Base(_Base): """ Base class to add orm functionality to all pycram mappings """ @@ -59,7 +60,7 @@ def process_metadata(self): tables""" -class MapperArgsMixin: +class MapperArgsMixin(MappedAsDataclass): """ MapperArgsMixin stores __mapper_args__ information for certain subclass-tables. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html @@ -72,14 +73,14 @@ def __mapper_args__(self): return {"polymorphic_identity": self.__tablename__} -class PositionMixin: +class PositionMixin(MappedAsDataclass): """ PositionMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - position_to_init: bool = False + position_to_init: bool = field(default=False, init=False) @declared_attr def position_id(self) -> Mapped[int]: @@ -90,14 +91,14 @@ def position(self): return relationship(Position.__tablename__, init=False) -class QuaternionMixin: +class QuaternionMixin(MappedAsDataclass): """ QuaternionMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - orientation_to_init: bool = False + orientation_to_init: bool = field(default=False, init=False) @declared_attr def orientation_id(self) -> Mapped[int]: @@ -108,14 +109,14 @@ def orientation(self): return relationship(Quaternion.__tablename__, init=False) -class PoseMixin: +class PoseMixin(MappedAsDataclass): """ PoseMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/20/orm/declarative_mixins.html """ __abstract__ = True - pose_to_init: bool = False + pose_to_init: bool = field(default=False, init=False) @declared_attr def pose_id(self) -> Mapped[int]: @@ -126,7 +127,7 @@ def pose(self): return relationship(Pose.__tablename__, init=False) -class ProcessMetaData(MappedAsDataclass, _Base): +class ProcessMetaData(_Base): """ ProcessMetaData stores information about the context of this experiment. diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 3f2d64205..4c2bb6c4d 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -1,19 +1,20 @@ +from dataclasses import field from typing import Optional from pycram.orm.base import Base, MapperArgsMixin, PoseMixin, Pose -from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship +from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship, MappedAsDataclass from sqlalchemy import ForeignKey from ..enums import ObjectType -class ObjectMixin: +class ObjectMixin(MappedAsDataclass): """ ObjectMixin holds a foreign key column and its relationship to the referenced table. For information about Mixins, see https://docs.sqlalchemy.org/en/13/orm/extensions/declarative/mixins.html """ __abstract__ = True - object_to_init: bool = False + object_to_init: bool = field(default=False, init=False) @declared_attr def object_id(self) -> Mapped[int]: