From 49e6b6924382fb687bc938cc3dfad82d6af02ba9 Mon Sep 17 00:00:00 2001 From: Steve Harris Date: Sun, 6 Feb 2022 13:33:38 -0800 Subject: [PATCH 01/30] Draft of face following capability --- gaia_bot/buzzer/buzzer/__init__.py | 0 gaia_bot/buzzer/package.xml | 18 ++ gaia_bot/buzzer/resource/buzzer | 0 gaia_bot/buzzer/setup.cfg | 4 + gaia_bot/buzzer/setup.py | 25 +++ gaia_bot/buzzer/test/test_copyright.py | 23 ++ gaia_bot/buzzer/test/test_flake8.py | 25 +++ gaia_bot/buzzer/test/test_pep257.py | 23 ++ gaia_bot/faces/faces/faces.py | 2 +- gaia_bot/gaia_bot/CONTRIBUTING.md | 13 ++ gaia_bot/gaia_bot/LICENSE | 202 +++++++++++++++++ .../gaia_bot/include/gaia_bot/gaia_bot.hpp | 7 + gaia_bot/gaia_bot/launch/full.launch.py | 8 +- gaia_bot/gaia_bot/src/gaia_bot.cpp | 208 ++++++++++++------ gaia_bot/gaia_bot/src/gaia_bot.ddl | 61 ++++- gaia_bot/gaia_bot/src/gaia_bot.ruleset | 141 +++++++++++- gaia_bot/gaia_bot_interfaces/CMakeLists.txt | 31 +++ gaia_bot/gaia_bot_interfaces/msg/Leds.msg | 9 + .../gaia_bot_interfaces/msg/WheelSpeeds.msg | 8 + gaia_bot/gaia_bot_interfaces/package.xml | 24 ++ gaia_bot/line/line/__init__.py | 0 gaia_bot/line/package.xml | 18 ++ gaia_bot/line/resource/line | 0 gaia_bot/line/setup.cfg | 4 + gaia_bot/line/setup.py | 25 +++ gaia_bot/line/test/test_copyright.py | 23 ++ gaia_bot/line/test/test_flake8.py | 25 +++ gaia_bot/line/test/test_pep257.py | 23 ++ gaia_bot/pca9685/pca9685/neck_pose.py | 2 +- 29 files changed, 867 insertions(+), 85 deletions(-) create mode 100644 gaia_bot/buzzer/buzzer/__init__.py create mode 100644 gaia_bot/buzzer/package.xml create mode 100644 gaia_bot/buzzer/resource/buzzer create mode 100644 gaia_bot/buzzer/setup.cfg create mode 100644 gaia_bot/buzzer/setup.py create mode 100644 gaia_bot/buzzer/test/test_copyright.py create mode 100644 gaia_bot/buzzer/test/test_flake8.py create mode 100644 gaia_bot/buzzer/test/test_pep257.py create mode 100644 gaia_bot/gaia_bot/CONTRIBUTING.md create mode 100644 gaia_bot/gaia_bot/LICENSE create mode 100644 gaia_bot/gaia_bot_interfaces/CMakeLists.txt create mode 100644 gaia_bot/gaia_bot_interfaces/msg/Leds.msg create mode 100644 gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg create mode 100644 gaia_bot/gaia_bot_interfaces/package.xml create mode 100644 gaia_bot/line/line/__init__.py create mode 100644 gaia_bot/line/package.xml create mode 100644 gaia_bot/line/resource/line create mode 100644 gaia_bot/line/setup.cfg create mode 100644 gaia_bot/line/setup.py create mode 100644 gaia_bot/line/test/test_copyright.py create mode 100644 gaia_bot/line/test/test_flake8.py create mode 100644 gaia_bot/line/test/test_pep257.py diff --git a/gaia_bot/buzzer/buzzer/__init__.py b/gaia_bot/buzzer/buzzer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gaia_bot/buzzer/package.xml b/gaia_bot/buzzer/package.xml new file mode 100644 index 0000000..8605f92 --- /dev/null +++ b/gaia_bot/buzzer/package.xml @@ -0,0 +1,18 @@ + + + + buzzer + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/gaia_bot/buzzer/resource/buzzer b/gaia_bot/buzzer/resource/buzzer new file mode 100644 index 0000000..e69de29 diff --git a/gaia_bot/buzzer/setup.cfg b/gaia_bot/buzzer/setup.cfg new file mode 100644 index 0000000..3748e0a --- /dev/null +++ b/gaia_bot/buzzer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/buzzer +[install] +install_scripts=$base/lib/buzzer diff --git a/gaia_bot/buzzer/setup.py b/gaia_bot/buzzer/setup.py new file mode 100644 index 0000000..c8067b3 --- /dev/null +++ b/gaia_bot/buzzer/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'buzzer' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='steve@gaiaplatform.io', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/gaia_bot/buzzer/test/test_copyright.py b/gaia_bot/buzzer/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/gaia_bot/buzzer/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/gaia_bot/buzzer/test/test_flake8.py b/gaia_bot/buzzer/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/gaia_bot/buzzer/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/gaia_bot/buzzer/test/test_pep257.py b/gaia_bot/buzzer/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/gaia_bot/buzzer/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/gaia_bot/faces/faces/faces.py b/gaia_bot/faces/faces/faces.py index 3454cee..be5abb9 100644 --- a/gaia_bot/faces/faces/faces.py +++ b/gaia_bot/faces/faces/faces.py @@ -46,7 +46,7 @@ def listener_callback(self, data): for (x, y, w, h) in detected_faces: face = Detection3D() face.bbox.center.position.x = (2 * x + w) / data.width - 1.0 - face.bbox.center.position.y = -1.0 * ((2 * y + h) / data.height - 1.0) + face.bbox.center.position.y = (2 * y + h) / data.height - 1.0 face.bbox.center.position.z = 0.0 face.bbox.size.x = float(2 * w / data.width) face.bbox.size.y = float(2 * h / data.height) diff --git a/gaia_bot/gaia_bot/CONTRIBUTING.md b/gaia_bot/gaia_bot/CONTRIBUTING.md new file mode 100644 index 0000000..6f63de9 --- /dev/null +++ b/gaia_bot/gaia_bot/CONTRIBUTING.md @@ -0,0 +1,13 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ diff --git a/gaia_bot/gaia_bot/LICENSE b/gaia_bot/gaia_bot/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/gaia_bot/gaia_bot/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/gaia_bot/gaia_bot/include/gaia_bot/gaia_bot.hpp b/gaia_bot/gaia_bot/include/gaia_bot/gaia_bot.hpp index 1b89193..6121b48 100644 --- a/gaia_bot/gaia_bot/include/gaia_bot/gaia_bot.hpp +++ b/gaia_bot/gaia_bot/include/gaia_bot/gaia_bot.hpp @@ -46,6 +46,8 @@ class GaiaBot : public rclcpp::Node static void publish_neck_pose(float rotation, float lift); private: + static constexpr std::chrono::milliseconds c_ego_time_update_interval = std::chrono::milliseconds(50); + rclcpp::Publisher::SharedPtr m_joint_trajectory_pub{}; rclcpp::Subscription::SharedPtr m_battery_state_sub{}; @@ -54,6 +56,11 @@ class GaiaBot : public rclcpp::Node rclcpp::Subscription::SharedPtr m_range_sub{}; rclcpp::Subscription::SharedPtr m_face_detection_sub{}; + static bool s_running; + + /// \brief Periodically update ego time. + static void ego_time_update(); + /// \brief Handle incoming messages. void left_light_callback(const sensor_msgs::msg::Illuminance::ConstSharedPtr msg); diff --git a/gaia_bot/gaia_bot/launch/full.launch.py b/gaia_bot/gaia_bot/launch/full.launch.py index 4da69b3..f381748 100644 --- a/gaia_bot/gaia_bot/launch/full.launch.py +++ b/gaia_bot/gaia_bot/launch/full.launch.py @@ -87,9 +87,9 @@ def generate_launch_description(): gaia_bot_container, camera, faces, - leds, - range, - sensors, - wheels, +# leds, +# range, +# sensors, +# wheels, neck_pose, ]) diff --git a/gaia_bot/gaia_bot/src/gaia_bot.cpp b/gaia_bot/gaia_bot/src/gaia_bot.cpp index 0ad45da..fc6a9e5 100644 --- a/gaia_bot/gaia_bot/src/gaia_bot.cpp +++ b/gaia_bot/gaia_bot/src/gaia_bot.cpp @@ -4,7 +4,7 @@ // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // -//    http://www.apache.org/licenses/LICENSE-2.0 +// http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, @@ -16,9 +16,21 @@ #include "gaia/logger.hpp" #include - +#include + +using std::chrono::duration_cast; +using std::chrono::milliseconds; +using std::chrono::system_clock; +using std::thread; +using std::this_thread::sleep_for; +using gaia::db::begin_transaction; +using gaia::db::commit_transaction; using gaia::gaia_bot::ego_t; +using gaia::gaia_bot::neck_pose_t; +using gaia::gaia_bot::neck_pose_writer; using gaia::gaia_bot::range_sensors_t; +using gaia::gaia_bot::face_detections_t; +using gaia::gaia_bot::face_tracks_t; using sensor_msgs::msg::BatteryState; using sensor_msgs::msg::Range; using sensor_msgs::msg::Illuminance; @@ -29,124 +41,178 @@ using trajectory_msgs::msg::JointTrajectoryPoint; namespace gaia_bot { +bool GaiaBot::s_running = true; + +uint64_t get_time_millis() +{ + return duration_cast(system_clock::now().time_since_epoch()).count(); +} + GaiaBot::GaiaBot(const rclcpp::NodeOptions & options) : Node("gaia_bot", options) { - // Make this Node instance available for the ruleset. - m_ptr.reset(this); + // Make this Node instance available for the ruleset. + m_ptr.reset(this); - // Initialize Gaia. - gaia::system::initialize(); + // Initialize Gaia. + gaia::system::initialize(); - rclcpp::on_shutdown( - [this] - { - GaiaBot::shutdown_callback(); - }); + rclcpp::on_shutdown( + [this] + { + GaiaBot::shutdown_callback(); + }); - // Initialize database. - gaia::db::begin_transaction(); - if (ego_t::list().size() == 0) - { - ego_t::insert_row(0); - } - gaia::db::commit_transaction(); + // Create publishers and subscribers. + using std::placeholders::_1; - // Create publishers and subscribers. - using std::placeholders::_1; + m_joint_trajectory_pub = create_publisher("neck_pose", rclcpp::QoS(10)); - m_joint_trajectory_pub = create_publisher("neck_pose", rclcpp::QoS(10)); + m_left_light_sub = create_subscription( + "left_light", rclcpp::QoS(10), + std::bind(&GaiaBot::left_light_callback, this, _1)); - m_left_light_sub = create_subscription( - "left_light", rclcpp::QoS(10), - std::bind(&GaiaBot::left_light_callback, this, _1)); + m_right_light_sub = create_subscription( + "right_light", rclcpp::QoS(10), + std::bind(&GaiaBot::right_light_callback, this, _1)); - m_right_light_sub = create_subscription( - "right_light", rclcpp::QoS(10), - std::bind(&GaiaBot::right_light_callback, this, _1)); + m_battery_state_sub = create_subscription( + "battery", rclcpp::QoS(10), + std::bind(&GaiaBot::battery_state_callback, this, _1)); - m_battery_state_sub = create_subscription( - "battery", rclcpp::QoS(10), - std::bind(&GaiaBot::battery_state_callback, this, _1)); + m_range_sub = create_subscription( + "range", rclcpp::QoS(10), + std::bind(&GaiaBot::range_callback, this, _1)); - m_range_sub = create_subscription( - "range", rclcpp::QoS(10), - std::bind(&GaiaBot::range_callback, this, _1)); + m_face_detection_sub = create_subscription( + "face", rclcpp::QoS(10), + std::bind(&GaiaBot::face_detection_callback, this, _1)); - m_face_detection_sub = create_subscription( - "face", rclcpp::QoS(10), - std::bind(&GaiaBot::face_detection_callback, this, _1)); + // Initialize database. + begin_transaction(); + if (ego_t::list().size() == 0) + { + auto ego = ego_t::get(ego_t::insert_row(1, "gaia_bot", get_time_millis())); + neck_pose_t::insert_row(0.0, 0.0); + } + else + { + auto neck_pose = *neck_pose_t::list().begin(); + if (neck_pose) + { + neck_pose_writer w = neck_pose.writer(); + w.rotation = 0.0; + w.lift = 0.0; + w.update_row(); + } + } + commit_transaction(); + + thread ego_time_update_thread(ego_time_update); + ego_time_update_thread.detach(); } GaiaBot::~GaiaBot() { - m_ptr.release(); + m_ptr.release(); +} + +void GaiaBot::ego_time_update() +{ + gaia::system::initialize(); + + while (s_running) + { + begin_transaction(); + auto ego_iter = ego_t::list().where(ego_t::expr::id == 1).begin(); + if (ego_iter != ego_t::list().end()) + { + auto ego_writer = ego_iter->writer(); + ego_writer.now = get_time_millis(); + ego_writer.update_row(); + } + commit_transaction(); + + sleep_for(c_ego_time_update_interval); + } } void GaiaBot::publish_neck_pose(float rotation, float lift) { - JointTrajectory msg; + JointTrajectory msg; + + msg.joint_names.push_back("rotation"); + msg.joint_names.push_back("lift"); - msg.joint_names.push_back("rotation"); - msg.joint_names.push_back("lift"); + JointTrajectoryPoint jtpRotation; + jtpRotation.positions.push_back(rotation); - JointTrajectoryPoint jtpRotation; - jtpRotation.positions.push_back(rotation); + JointTrajectoryPoint jtpLift; + jtpLift.positions.push_back(lift); - JointTrajectoryPoint jtpLift; - jtpLift.positions.push_back(lift); - - msg.points.push_back(jtpRotation); - msg.points.push_back(jtpLift); + msg.points.push_back(jtpRotation); + msg.points.push_back(jtpLift); - m_ptr->m_joint_trajectory_pub->publish(msg); + m_ptr->m_joint_trajectory_pub->publish(msg); } void GaiaBot::left_light_callback(const Illuminance::ConstSharedPtr msg) { - gaia_log::app().info("Left light Illuminance {}", msg->illuminance); - // TODO: Handle left light illuminance + gaia_log::app().info("Left light Illuminance {}", msg->illuminance); + // TODO: Handle left light illuminance } void GaiaBot::right_light_callback(const Illuminance::ConstSharedPtr msg) { - gaia_log::app().info("Right light Illuminance {}", msg->illuminance); - // TODO: Handle right light illuminance + gaia_log::app().info("Right light Illuminance {}", msg->illuminance); + // TODO: Handle right light illuminance } void GaiaBot::battery_state_callback(const BatteryState::ConstSharedPtr msg) { - gaia_log::app().info("BatteryState voltage {}", msg->voltage); - // TODO: Handle battery state + gaia_log::app().info("BatteryState voltage {}", msg->voltage); + // TODO: Handle battery state } void GaiaBot::range_callback(const Range::ConstSharedPtr msg) { - gaia::db::begin_transaction(); - // TODO: Generalize to handle multiple range sensors - auto range_sensor_iter = range_sensors_t::list().where(range_sensors_t::expr::id == 0).begin(); - if (range_sensor_iter == range_sensors_t::list().end()) - { - range_sensors_t::insert_row(0, "main_range_sensor", msg->range, 0); - } - else - { - auto range_sensor_writer = range_sensor_iter->writer(); - range_sensor_writer.distance = msg->range; - range_sensor_writer.update_row(); - } - gaia::db::commit_transaction(); + begin_transaction(); + // TODO: Generalize to handle multiple range sensors + auto range_sensor_iter = range_sensors_t::list().where(range_sensors_t::expr::id == 0).begin(); + if (range_sensor_iter == range_sensors_t::list().end()) + { + range_sensors_t::insert_row(0, "main_range_sensor", get_time_millis(), msg->range, 1); + } + else + { + auto range_sensor_writer = range_sensor_iter->writer(); + range_sensor_writer.distance = msg->range; + range_sensor_writer.timestamp = get_time_millis(); + range_sensor_writer.update_row(); + } + commit_transaction(); } void GaiaBot::face_detection_callback(const Detection3D::ConstSharedPtr msg) { - gaia_log::app().info("Detection Message {}, {}", msg->bbox.center.position.x, msg->bbox.center.position.y); - // TODO: Handle face detections + try + { + begin_transaction(); + face_detections_t::insert_row( + get_time_millis(), msg->bbox.center.position.x, msg->bbox.center.position.y, msg->bbox.center.position.z, + msg->bbox.size.x, msg->bbox.size.y, msg->bbox.size.z, false, 0); + commit_transaction(); + } + catch (const gaia::db::transaction_update_conflict& ex) + { + gaia_log::app().error("transaction_update_conflict"); + } } void GaiaBot::shutdown_callback() { - gaia::system::shutdown(); + s_running = false; + gaia::system::shutdown(); } } // namespace gaia_bot diff --git a/gaia_bot/gaia_bot/src/gaia_bot.ddl b/gaia_bot/gaia_bot/src/gaia_bot.ddl index dc2c4a5..7855b4b 100644 --- a/gaia_bot/gaia_bot/src/gaia_bot.ddl +++ b/gaia_bot/gaia_bot/src/gaia_bot.ddl @@ -2,18 +2,75 @@ database gaia_bot table ego ( id uint8 unique, + name string, + + now uint64, range_sensors - references range_sensors[] + references range_sensors[], + face_tracks + references face_tracks[] +) + +table neck_pose ( + rotation float, + lift float ) table range_sensors ( id uint8, name string, + + timestamp uint64, distance uint16, ego references ego where range_sensors.ego_id = ego.id, ego_id uint8 -); +) + +table face_tracks ( + id uint32 unique, + name string, + + start_timestamp uint64, + last_timestamp uint64, + + dx float, + dy float, + dz float, + + face_detections references face_detections[], + detection_count uint16, + + remove_flag bool, + + ego + references ego + where face_tracks.ego_id = ego.id, + ego_id uint8 +) + +table face_detections ( + timestamp uint64, + + -- location of face relative to current field of view of camera + -- (0,0,0) is center, (-1,-1,0) is upper left, (1,1,0) is lower right + -- dz is currently always 0 + dx float, + dy float, + dz float, + + width float, + height float, + depth float, + + remove_flag bool, + + face_track + references face_tracks + where face_detections.face_track_id = face_tracks.id, + face_track_id uint32 +) +; diff --git a/gaia_bot/gaia_bot/src/gaia_bot.ruleset b/gaia_bot/gaia_bot/src/gaia_bot.ruleset index e0766f4..600b61b 100644 --- a/gaia_bot/gaia_bot/src/gaia_bot.ruleset +++ b/gaia_bot/gaia_bot/src/gaia_bot.ruleset @@ -18,13 +18,142 @@ #include #include -using gaia_bot::GaiaBot; +// using gaia_bot::GaiaBot::publish_neck_pose; using sensor_msgs::msg::Range; +using namespace gaia::gaia_bot; -ruleset gaia_bot +const uint64_t face_track_timeout = 3000; +const float look_range = 0.01; +const float neck_move_step = 0.005; +// const uint64_t face_detection_timeout = 5000; + +ruleset gaia_bot: serial_group() { - on_change(range_sensors.distance) - { - gaia_log::app().info("range_sensor distance changed {}", range_sensors.distance); - } + on_update(ego.now) + { + for (/FT:face_tracks) if (!FT.remove_flag) + { + uint64_t last_timestamp_new; + last_timestamp_new = FT.start_timestamp; + uint16_t detection_count_new = 0; + float dx_new = 0.0; + float dy_new = 0.0; + + for (FT->FD:face_detections) + { + if (FD.timestamp > last_timestamp_new) + { + last_timestamp_new = FD.timestamp; + } + detection_count_new++; + dx_new += FD.dx; + dy_new += FD.dy; + } + + FT.detection_count = detection_count_new; + + if (detection_count_new == 0) + { + gaia_log::app().info("Setting remove flag for FT, existing flag {}", FT.remove_flag); + + FT.remove_flag = true; + } + else if (last_timestamp_new + face_track_timeout > ego.now) + { + FT.last_timestamp = last_timestamp_new; + + // detection_count_new should never be less than 1 + dx_new /= detection_count_new; + dy_new /= detection_count_new; + FT.dx = dx_new; + FT.dy = dy_new; + } + else + { + for (FT->FD:face_detections) + { + FD.remove_flag = true; + } + } + + if (detection_count_new > 10) + { + // gaia_log::app().info("face_track: {}, detections {}", FT.id, FT.detection_count); + for (/NP:neck_pose) + { + gaia_log::app().info("dx_new: {}, NP.rotation {}", dx_new, NP.rotation); + if (dx_new > look_range) + { + NP.rotation += neck_move_step; + } + else if (dx_new < -look_range) + { + NP.rotation -= neck_move_step; + } + if (dy_new > look_range) + { + NP.lift -= neck_move_step; + } + else if (dy_new < look_range) + { + NP.lift += neck_move_step; + } + } + } + } + } + + on_change(range_sensors.distance) + { + gaia_log::app().info("range_sensor distance changed {}", range_sensors.distance); + } + + on_change(neck_pose) + { + gaia_log::app().info("neck_pose rotation:{}, lift:{}", neck_pose.rotation, neck_pose.lift); + gaia_bot::GaiaBot::publish_neck_pose(neck_pose.rotation, neck_pose.lift); + } + + on_insert(FD:face_detections) + { + // gaia_log::app().info("face detection {}, {}, {}, {}, {}, {}", FD.dx, FD.dy, FD.dz, FD.width, FD.height, FD.depth); + + uint32_t face_tracks_id_max = 0; + float dx_min, dx_max, dy_min, dy_max; + dx_min = FD.dx - (FD.width / 2); + dx_max = dx_min + FD.width; + dy_min = FD.dy - (FD.height / 2); + dy_max = dy_min + FD.height; + + for (/FT:face_tracks) + { + if (face_tracks_id_max < FT.id) + { + face_tracks_id_max = FT.id; + } + if (!FT.remove_flag && FT.dx > dx_min && FT.dx < dx_max && FT.dy > dy_min && FT.dy < dy_max) + { + FD.face_track_id = FT.id; + return; + } + } + + face_tracks_id_max++; + gaia_log::app().info("inserting face_track {}", face_tracks_id_max); + face_tracks.insert(id:face_tracks_id_max, name:"face_track_name", start_timestamp:FD.timestamp, + last_timestamp:FD.timestamp, dx:FD.dx, dy:FD.dy, dz:FD.dz, detection_count:1, ego_id:1); + FD.face_track_id = face_tracks_id_max; + } + + on_update(face_tracks.remove_flag) + { + face_tracks.ego_id = 0; + face_tracks.remove(); + } + + on_update(face_detections.remove_flag) + { + face_detections.face_track_id = 0; + face_detections.remove(); + } } diff --git a/gaia_bot/gaia_bot_interfaces/CMakeLists.txt b/gaia_bot/gaia_bot_interfaces/CMakeLists.txt new file mode 100644 index 0000000..82b9030 --- /dev/null +++ b/gaia_bot/gaia_bot_interfaces/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(gaia_bot_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/WheelSpeeds.msg" + "msg/Leds.msg" + DEPENDENCIES + std_msgs + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/gaia_bot/gaia_bot_interfaces/msg/Leds.msg b/gaia_bot/gaia_bot_interfaces/msg/Leds.msg new file mode 100644 index 0000000..94e58d6 --- /dev/null +++ b/gaia_bot/gaia_bot_interfaces/msg/Leds.msg @@ -0,0 +1,9 @@ +# LED color + +std_msgs/Header header + +uint32 led_bmp # bitmap of applicable LEDs + +uint8 red +uint8 green +uint8 blue \ No newline at end of file diff --git a/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg b/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg new file mode 100644 index 0000000..b22082c --- /dev/null +++ b/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg @@ -0,0 +1,8 @@ +# Speeds for each wheel normalized to the range of -100 to 100 + +std_msgs/Header header + +float32 front_left +float32 front_right +float32 rear_left +float32 rear_right \ No newline at end of file diff --git a/gaia_bot/gaia_bot_interfaces/package.xml b/gaia_bot/gaia_bot_interfaces/package.xml new file mode 100644 index 0000000..949ecf5 --- /dev/null +++ b/gaia_bot/gaia_bot_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + gaia_bot_interfaces + 0.0.0 + TODO: Package description + ubuntu + Apache 2.0 + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/gaia_bot/line/line/__init__.py b/gaia_bot/line/line/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gaia_bot/line/package.xml b/gaia_bot/line/package.xml new file mode 100644 index 0000000..4fac6a1 --- /dev/null +++ b/gaia_bot/line/package.xml @@ -0,0 +1,18 @@ + + + + line + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/gaia_bot/line/resource/line b/gaia_bot/line/resource/line new file mode 100644 index 0000000..e69de29 diff --git a/gaia_bot/line/setup.cfg b/gaia_bot/line/setup.cfg new file mode 100644 index 0000000..f597c3c --- /dev/null +++ b/gaia_bot/line/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/line +[install] +install_scripts=$base/lib/line diff --git a/gaia_bot/line/setup.py b/gaia_bot/line/setup.py new file mode 100644 index 0000000..60632ff --- /dev/null +++ b/gaia_bot/line/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'line' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='steve@gaiaplatform.io', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/gaia_bot/line/test/test_copyright.py b/gaia_bot/line/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/gaia_bot/line/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/gaia_bot/line/test/test_flake8.py b/gaia_bot/line/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/gaia_bot/line/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/gaia_bot/line/test/test_pep257.py b/gaia_bot/line/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/gaia_bot/line/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/gaia_bot/pca9685/pca9685/neck_pose.py b/gaia_bot/pca9685/pca9685/neck_pose.py index 6e8354c..f391583 100644 --- a/gaia_bot/pca9685/pca9685/neck_pose.py +++ b/gaia_bot/pca9685/pca9685/neck_pose.py @@ -25,7 +25,7 @@ def __init__(self): self.PwmServo.setServoPulse(9,1500) def setServoPwm(self,channel,angle,error=10): - angle=int(angle) + angle=int(angle * 90.0) + 90 if channel=='rotation': self.PwmServo.setServoPulse(8,2500-int((angle+error)/0.09)) elif channel=='lift': From 5c15f5cfc08c6df87d78c7a06a234b8e90084e6e Mon Sep 17 00:00:00 2001 From: Steve Harris Date: Thu, 17 Feb 2022 13:15:57 -0800 Subject: [PATCH 02/30] Minor cleanup --- gaia_bot/gaia_bot/launch/full.launch.py | 8 ++++---- gaia_bot/gaia_bot/src/gaia_bot.ruleset | 2 -- gaia_bot/gaia_bot_interfaces/msg/Leds.msg | 2 +- gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg | 2 +- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/gaia_bot/gaia_bot/launch/full.launch.py b/gaia_bot/gaia_bot/launch/full.launch.py index f381748..4da69b3 100644 --- a/gaia_bot/gaia_bot/launch/full.launch.py +++ b/gaia_bot/gaia_bot/launch/full.launch.py @@ -87,9 +87,9 @@ def generate_launch_description(): gaia_bot_container, camera, faces, -# leds, -# range, -# sensors, -# wheels, + leds, + range, + sensors, + wheels, neck_pose, ]) diff --git a/gaia_bot/gaia_bot/src/gaia_bot.ruleset b/gaia_bot/gaia_bot/src/gaia_bot.ruleset index 600b61b..433d7b4 100644 --- a/gaia_bot/gaia_bot/src/gaia_bot.ruleset +++ b/gaia_bot/gaia_bot/src/gaia_bot.ruleset @@ -18,14 +18,12 @@ #include #include -// using gaia_bot::GaiaBot::publish_neck_pose; using sensor_msgs::msg::Range; using namespace gaia::gaia_bot; const uint64_t face_track_timeout = 3000; const float look_range = 0.01; const float neck_move_step = 0.005; -// const uint64_t face_detection_timeout = 5000; ruleset gaia_bot: serial_group() { diff --git a/gaia_bot/gaia_bot_interfaces/msg/Leds.msg b/gaia_bot/gaia_bot_interfaces/msg/Leds.msg index 94e58d6..f0f1ef0 100644 --- a/gaia_bot/gaia_bot_interfaces/msg/Leds.msg +++ b/gaia_bot/gaia_bot_interfaces/msg/Leds.msg @@ -6,4 +6,4 @@ uint32 led_bmp # bitmap of applicable LEDs uint8 red uint8 green -uint8 blue \ No newline at end of file +uint8 blue diff --git a/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg b/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg index b22082c..216c314 100644 --- a/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg +++ b/gaia_bot/gaia_bot_interfaces/msg/WheelSpeeds.msg @@ -5,4 +5,4 @@ std_msgs/Header header float32 front_left float32 front_right float32 rear_left -float32 rear_right \ No newline at end of file +float32 rear_right From 4b732132b4b0f651882eac540269378297d8bcd9 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 30 Mar 2022 18:07:07 -0700 Subject: [PATCH 03/30] Outline of rearchitected ddl and rules --- slam_sim/gaia/Makefile | 32 ++-- slam_sim/gaia/slam.ddl | 345 ++++++++++++++++++------------------- slam_sim/gaia/slam.ruleset | 258 +++++++-------------------- 3 files changed, 241 insertions(+), 394 deletions(-) diff --git a/slam_sim/gaia/Makefile b/slam_sim/gaia/Makefile index 6233bbf..60b7256 100644 --- a/slam_sim/gaia/Makefile +++ b/slam_sim/gaia/Makefile @@ -1,32 +1,22 @@ -include ../Makefile.inc +# Copyright (c) Gaia Platform LLC +# +# Use of this source code is governed by the MIT +# license that can be found in the LICENSE.txt file +# or at https://opensource.org/licenses/MIT. + +# The purpose of this makefile is to simply execute gaiac and gaiat +# to make sure there are not errors (without executing higher level +# make). The single target then cleans up after itself. RULES_CPP = slam_rules.cpp DB_NAME = slam +INC = -I/opt/gaia/include/ -Islam/ -I../include/ OBJS = slam_rules.o slam/gaia_slam.o -all: rules all_procedure_subshell - -gaiac: +all: gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME) - -rules: gaiac gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) - -# when freshly making, rules.cpp may not exist (e.g., if 'make clean' -# was just run). drop into subshell for making local procedural files -# as that will induce a scan of local files -all_procedure_subshell: - make all_procedure - -all_procedure: $(OBJS) - -refresh: clean all - -%.o: %.cpp - $(CXX) $(INC) -c $(CPPFLAGS) $< - -clean: rm -f *.o $(RULES_CPP) rm -rf $(DB_NAME) diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 509a088..641a5ab 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -28,30 +28,57 @@ database slam; --- unit suffixes --- meters --- degs (degress) --- sec (seconds) --- pct (percentage) +-- Table list +-- Single record tables: +-- ego Stores state of the bot. +-- area_map Map of the known world. +-- working_map Version of world map with recent sensor overlay. +-- destination Location that bot is moving to. +-- +-- Event tables: +-- pending_destination Destination requested externally. +-- collision_event Sensors detected imminent collision. +-- +-- Data tables: +-- graphs Group of observations to produce pose graph. +-- observations Synonymous with node, pose, vertex. +-- positions Position associated with observation. +-- movements Movement to reach this observation. +-- range_data Sensor range data for an observation. +-- edges Connects two observations. +-- error_corrections Information from graph optimization round. ------------------------------------------------------------------------ -- Tables with single records (i.e., exactly one) +-- Record for the bot, storing its state and references to information. table ego ( - current_path_id uint32, - current_path references paths where ego.current_path_id=paths.id, + -- Reference to the active graph. Unlike the references below, this is + -- a value-linked (implicit) reference that is automatically updated + -- by updating the 'current_graph_id'. + current_graph_id uint32, + current_graph references graphs + where ego.current_graph_id = graphs.id, -- Explicitly created references. -- Keep most of ego data in different tables so that updates to -- one field don't risk conflict and txn rollback. - position references estimated_position, + -- The references here are for conveninece, as once the 'ego' record + -- is held there's a direct link to the other tables. Because those + -- other tables have only one record, they can easily be retrieved + -- directly (as an alternative to using references). + latest_observation references latest_observation, + + -- Position oriented. + world references observed_area destination references destination, - error references error_correction, + world references observed_area, + -- Map oriented. + world references observed_area low_res_map references area_map, - high_res_map references local_map, working_map references working_map ) @@ -61,6 +88,10 @@ table ego -- manage concurrent access manually. table area_map ( + // Reference to in-memory blob that stores 2D map. + // Blob ID changes on each map update. + uint32_t blob_id; + -- Bounding polygon -- Uses world coordinates, with increasing X,Y being rightward/upward. left_meters float, @@ -68,18 +99,17 @@ table area_map top_meters float, bottom_meters float, - -- An on_change rule needs a field to change to fire the rule. Use this - -- field to guarantee that a change is made so the rule is fired. Note - -- that a record being "touched" is not sufficient to trigger a change - -- rule as an actual value change is required. - change_counter int32, - ---------------------------- ego references ego + working_map references working_map ) -table local_map +table working_map ( + // Reference to in-memory blob that stores 2D map. + // Blob ID changes on each map update. + uint32_t blob_id; + -- Bounding polygon -- Uses world coordinates, with increasing X,Y being rightward/upward left_meters float, @@ -87,27 +117,9 @@ table local_map top_meters float, bottom_meters float, - -- An on_change rule needs a field to change to fire the rule. Use this - -- field to guarantee that a change is made so the rule is fired. Note - -- that a record being "touched" is not sufficient to trigger a change - -- rule as an actual value change is required. - change_counter int32, - ---------------------------- + -- References ego references ego, - working_map references working_map -) - - -table working_map -( - -- An on_change rule needs a field to change to fire the rule. Use this - -- field to guarantee that a change is made so the rule is fired. Note - -- that a record being "touched" is not sufficient to trigger a change - -- rule as an actual value change is required. - change_counter int32, - ---------------------------- - local_map references local_map, - ego references ego + area_map references area_map ) @@ -117,40 +129,26 @@ table destination x_meters float, y_meters float, - -- Expected arrival time, in numbers of observations to be taken - -- on this path. This is useful for aborting trying to reach a - -- destination. - -- When set to -1, this means return to a landmark. - expected_arrival int32, - ---------------------------- - ego references ego -) + -- Keep track of when we left for this destination. If too much time + -- has elapsed looking for it, abort and go somewhere else. + departure_time_sec float, - -table estimated_position -( - -- Uses world coordinates, with increasing X,Y being rightward/upward. - x_meters float, - y_meters float, - - -- Intended/believed motion - dx_meters float, - dy_meters float, - - ---------------------------- + -- References ego references ego ) --- Parameters to estimate tracking errors. This is a running average --- as estimated over all paths. -table error_correction +-- Represents the most recently created observation. +-- In the context of evaluating the data model for possible transaction +-- conflicts, If multiple observations are not created simultaneously +-- (which they shouldn't be) then it's not possible to get a transaction +-- collision updating the single record in this table. +-- Represents the most recently created observation. +table latest_observation ( - drift_correction float, - forward_correction float, - correction_weight float, - - ---------------------------- + observation_id uint32, + observation references observations + where latest_observation.observation_id = observations.id, ego references ego ) @@ -177,159 +175,148 @@ table collision_event ------------------------------------------------------------------------ --- Data tables +-- Data tables (i.e., tables with multiple records) --- A sequence of observations between a known start and end point. --- Start/end points are at distinguishable landmarks. -table paths +-- A collection of observations. +table graphs ( id uint32 unique, - ego references ego[], + observations references observations[], + edges references edges[], - state int32, - - -- Observations that are part of this path. - num_observations uint32, - start_obs_id uint32, - latest_obs_id uint32, - first_observation references observations - where paths.start_obs_id = observations.id, - latest_observation references observations - where paths.latest_obs_id = observations.id + -- Reverse reference to ego (necessary until one-way references are + -- supported) + ego references ego[] ) +------------------------------------------------ +-- Observation related +-- TODO rename 'observations' to either 'nodes', 'poses', or 'vertices'. + -- An observation from a point in the world, including sensor readings --- from this point and a link to observations that occurred immediately --- before and after. --- This is more typically known as a 'Node' in SLAM. The --- name difference is because this represents the sensor data from --- an individual position only. It is not linked together to form --- a graph of an arbitrary number of nearby pose points, only the --- previous and next observations. In future iterations, a generic 'node' --- will be defined, based on their typical definition. It is expected --- that the node will be similar to an observation. +-- from this point and a link to other nearby observations. This is +-- more commonly known as a "node", "pose", or "vertex". +-- Most of observation data in kept different tables so that updates to +-- one field don't risk conflict and txn rollback if others are +-- modified. table observations ( + ------------------------------ + -- Constant data. These values are set at creation and don't change. + -- This is relevant in the sense that, when evaluating for possible + -- transaction conflicts, these fields/references can be considered + -- safe. id uint32 unique, - -- Estimated position. This is the latest best guess. It may be modified - -- in the future when we have better error estimates. - pos_x_meters float, - pos_y_meters float, - - -- Actual position data. - actual_x_meters float, - actual_y_meters float, + position references positions, + range_data references range_data, + motion references movements, - -- DR motion from previous observation. - dx_meters float, - dy_meters float, - - -- DR motion in polar coordinates. - heading_degs float, - dist_meters float, - - - path references paths[] using latest_observation, - path_dup references paths[] using first_observation, + graph_id uint32, + graph references graphs + where graphs.id = observations.graph_id, ------------------------------ - -- Sensing - - -- Range finder - num_radials int32, - distance_meters float[], - - landmark_sightings references landmark_sightings[], - - -- As noted, an observation is only connected to two other observations. - -- A node would instead have a single 'references edges[]' - -- Forward is the edge connecting this observation to the next one, and - -- reverse connects this observation to the previous. - forward_edge references edges, - reverse_edge references edges + -- Dynamic fields. These can be modified asynchronously. Functions + -- and rules that modify them (i.e., that add edges) should be designed + -- to handle a possible transaction rollback unless the logic is + -- desgined to avoid that. + in_edges references edges[], + out_edges references edges[], + + -- Reverse reference to 'latest_observation'. This is not used and + -- can be removed when one-way references are supported. The [] + -- can be removed when 1:1 VLRs are supported. + prev_observation references latest_observation[] ) -table edges +table positions ( - -- ID is the same as that of target (next) observation. - id uint32, - - -- For sequential observations O1 and O2, this edge attaches to - -- O1.forward_edge and O2.reverse_edge, so 'next' is O2.reverse_edge - -- and 'prev' is O1.forward_edge. - next references observations using reverse_edge, - prev references observations using forward_edge + -- The latest best guess of position. It may be modified in the future + -- when we have better error estimates. + x_meters float, + y_meters float, + heading_degs float, + + -- Constant reference established at record creation time. + observation references observations ) --- A salient feature of the environment that is uniquely identifiable and --- that can be used to generate a position fix. -table landmarks +table movements ( - id uint32 unique, - description string unique, - - -- Position of landmark. If this is not supplied externally then it is - -- generated by Alice. If positions are supplied externally then - -- their confidence is 1.0. Otherwise, the first landmark encountered - -- will be assigned a position relative to Alice's position and a - -- confidence of 1.0. Subsequent landmarks will have confidences less - -- than 1.0 and will have approximate positions that are subject to - -- being updated. - x_meters float, - y_meters float, - - confidence float, + -- DR (dead reckoning) motion from previous observation. This is + -- a placeholder for storing DR data that could be used during + -- graph optimization. + dx_meters float, + dy_meters float, + dheading_degs float, - -- Times that this landmark was sighted. - sightings references landmark_sightings[] + -- Constant reference established at record creation time. + observation references observations ) --- Locations of observed landmarks. --- Note that measured distance and range to the landmark will be more --- accurate the closer Alice is to it. -table landmark_sightings +table range_data ( - range_meters float, - bearing_degs float, + -- This can be stored in a blob or in dedicated fields. + num_radials int32, + bearing_degs float[], + distance_meters float[], - -- Observation corresponding to this sighting. - observation_id uint32, + -- Constant reference established at record creation time. observation references observations - where landmark_sightings.observation_id = observations.id, - - -- Landmark that was sighted. - landmark_id uint32, - landmark references landmarks - where landmark_sightings.landmark_id = landmarks.id ) ------------------------------------------------------------------------- --- Used by simulator shell +------------------------------------------------ +-- Other supporting tables. --- Alice's initial position in the world is 0,0. This is the coordinate --- mapping to get from 0,0 to the physical start point. --- There is exactly one record in this table. -table sim_position_offset +-- Connects two observations. +table edges ( - -- Based on world coordinates, with increasing X,Y being rightward/upward - dx_meters float, - dy_meters float + graph_id uint32, + graph references graphs + where edges.graph_id = graphs.id, + + -- An edge connects two observations, source and destination. These + -- have names indicating that they are directed, as they sometimes + -- will be (observation X -> Y -> Z), but it is noted that many times + -- they won't be (e.g., if an edge is formed between observation Y + -- and existing observation M). + src_id uint32, + src references observations + using out_edges where edges.src_id = observations.id, + + dest_id uint32, + dest references observations + using in_edges where edges.dest_id = observations.id ) --- Alice's actual position in Alice's reference frame. --- There is exactly one record in this table. -table sim_actual_position +-- Created whenever an optimization is performed. Table can store data +-- from that optimization, but more relevantly creating this record also +-- generates as an event to do post-optimization things, like creating a +-- new map. +table error_corrections ( - -- World coordinates, with increasing X,Y being rightward/upward - x_meters float, - y_meters float + id uint32 unique, + graph_id uint32 + + -- A reference can be made to graphs if necesasry. However, if it's + -- unlikely to be used, or be used infrequently, might be better + -- to avoid having reference, to avoid possible transaction conflicts + -- if modifying graph_id in a transaction that has computationally + -- intensive actions associated with it. + -- In the absense of a reference, the correct graph can always be + -- reached using the following direct access approach: + -- graphs_t g = *(graphs_t::list() + -- .where(graphs_t::expr::id == ec.graph_id).begin()); + -- for error_corrections record 'ec'. + + -- Addional data fields here, as necessary. ) diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 0987787..96574d9 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -23,71 +23,81 @@ #include "slam_sim.hpp" -using slam_sim::select_destination; -using slam_sim::select_landmark_destination; -using slam_sim::full_stop; -using slam_sim::move_toward_destination; +//using slam_sim::select_destination; +//using slam_sim::select_landmark_destination; -using slam_sim::create_new_path; -using slam_sim::create_observation; -using slam_sim::init_path; - -using slam_sim::calc_path_error; -using slam_sim::build_area_map; -using slam_sim::build_local_map; -using slam_sim::build_working_map; +// Brings Alice to a halt. Creates new observation at present position. +using slam_sim::full_stop; -using gaia::slam::pending_destination_t; -using gaia::slam::estimated_position_t; -using gaia::slam::destination_t; -using gaia::slam::paths_t; +// Instructs Alice to beging trek to destination. Motion control is +// managed at lower level, using direction data determined in rules. +using slam_sim::move_toward_destination; -using gaia::slam::paths_writer; +//using slam_sim::create_new_path; +//using slam_sim::create_observation; +//using slam_sim::init_path; +// +//using slam_sim::calc_path_error; +//using slam_sim::build_area_map; +//using slam_sim::build_local_map; +//using slam_sim::build_working_map; +// +//using gaia::slam::pending_destination_t; +//using gaia::slam::estimated_position_t; +//using gaia::slam::destination_t; +//using gaia::slam::paths_t; +// +//using gaia::slam::paths_writer; -ruleset observation_ruleset +// Wrap error correction and map creation in a serial group as these are +// expensive operations and we want to avoid concurrent operations +// experiencing a txn conflict, and avoid race conditions. +ruleset map_ruleset : serial_group() { - //////////////////////////////////////////// - // Change of path state - - on_change(paths.state) + // We don't want multiple edges to be created at similar points in time + // and both realize that it's time to optimize the map, and do so + // together, so keep the graph optimization check in a serial group. + on_insert(e: edges) { - if ((state == slam_sim::PATH_STATE_STARTING) && (slam_sim::g_quit == 0)) + // New edge created. See if it's time to run new graph optimization. + if (optimization_required()) { - gaia_log::app().info("Path changed state to STARTING. " - "Creating initial observation"); - // Sanity check. A new path should have no observations. - uint32_t n = 0xffffffff; - n = paths.num_observations; - assert(n == 0); + optimize_graph(e.dest->observations.graph->graphs); - // Create first observation. That will trigger the sequence - // of events moving toward the destination. - create_observation(paths); + // Whenever new error-correction data is available, regenerate + // maps. This can be triggered in a separate rule or simply + // done here, so do it here. + build_area_map(/area_map, /observed_area); + build_working_map(/destination, /area_map); } } - on_update(paths.state) + // Destination changed. Update working map and start moving toward it. + on_change(d: destination) + { + build_working_map(d, /area_map); + move_toward_destination(/working_map); + } + + + // For each observation, update working map using new sensor data. + on_insert(o: observations) { - if (paths.state == slam_sim::PATH_STATE_DONE) + // If working map will go beyond border of area map, rebuild + // area map. + if (need_to_extend_area_map()) { - gaia_log::app().info("Path changed state to DONE. " - "Calculating path error"); - // Path just completed. Calculate error and store results - // in error_correction. There should be no transaction - // conflicts as this is the only code path altering - // the record in that table. Once error is calculated - // a new area map can be created. - // TODO make sure this is a complete path (i.e., w/ landmark - // at beginning and end) so that DR errors can be estimated - // from it. - calc_path_error(paths); - gaia_log::app().info("DONE Path changed state to DONE."); + build_area_map(/area_map, /observed_area); } + build_working_map(g, /area_map); } +} +ruleset observation_ruleset +{ //////////////////////////////////////////// // Creating observations @@ -98,166 +108,26 @@ ruleset observation_ruleset { gaia_log::app().info("Estimated position changed to {},{}", ep.x_meters, ep.y_meters); - create_observation(/ego.current_path->paths); - } - - - on_insert(o: observations) - { - // An observation of the surroundings was made. Now we need to - // move on towards our destination. - // There are several things to consider. Updating the destination - // record will trigger a rebuilding of the local maps, which will - // in turn have Alice move toward the destination. - -gaia_log::app().info("on_insert(observation);"); -gaia_log::app().info(" state={}", o.path->paths.state); - for (/pending_destination) { - // If there's a new destination request pending, set that as the - // destination. - gaia_log::app().info("Observation found pending destination " - "change request"); - - // TODO modify destination record - assert(1 == 0); // induce a fault until this is supported - - return; - } - - if (o.path->paths.state == slam_sim::PATH_STATE_STARTING) - { - gaia_log::app().info("Observation created in new path. " - "Selecting destination"); - // This is a newly created path. Select a destination. - // Add this observation to the path - init_path(o); - // Path initization changes the state to active. This change - // doesn't trigger any side actions, so there's nothing to - // worry about if we modify another row that will induce - // a cascade of rule executions. - // Select a destination. - select_destination(); - return; - } - - if (o.path->paths.state == slam_sim::PATH_STATE_ACTIVE) - { - // First observation in path made before a destination is selected. - // Defer check for expected arrival until we have a [new] - // destination. - if ((o.path->paths.num_observations > 1) && - (o.path->paths.num_observations > (uint32_t) expected_arrival)) - { - gaia_log::app().info("Observation made beyond expected " - "arrival time at destination. Returning to landmark"); - // We're exploring and should have reached our destination - // already. Give up and go back to a landmark. - select_landmark_destination(); - return; - } - } - - // If we're close enought to a landmark right now, terminate - // the path. - for (o.landmark_sightings->ls:landmark_sightings) { -gaia_log::app().info("Landmark sighting. Range {}", ls.range_meters); - if (ls.range_meters < slam_sim::LANDMARK_DISTANCE_METERS) { - gaia_log::app().info("Observation made made near " - "landmark. Terminating path"); - o.path->paths.state = slam_sim::PATH_STATE_DONE; - return; - } - } - // If position is sufficiently close to destination then stop - // exploring and go find a landmark (or if we arrived where one - // should be, go look for another one). - // Both destination and estimated position tables have exactly - // on record, so we can use a shortcut to access the fields - // in these records directly. - double dx = 0.0; - double dy = 0.0; - dx = /destination.x_meters - /estimated_position.x_meters; - dy = /destination.y_meters - /estimated_position.y_meters; - double dist_2 = dx*dx + dy*dy; - double radius_2 = slam_sim::DESTINATION_RADIUS_METERS - * slam_sim::DESTINATION_RADIUS_METERS; - if (dist_2 < radius_2) + // Determine if it's time to build another observation. + if (should_build_new_observation()) { - gaia_log::app().info("Observation made at destination. " - "Returning to landmark."); - // Arrived at destination. Find a landmark to calibrate - // position and then update map. - select_landmark_destination(); - return; + create_observation(/ego.current_path->paths); } - - // Destination hasn't changed. Rebuild the working map - // and proceed. - gaia_log::app().info("Observation made. Building new local map."); - build_working_map(/working_map); - } - - - //////////////////////////////////////////// - // Setting destination and error correction - - on_update(d: destination) - { - // make sure destination and location are w/in map...... - // TODO FIXME using high-res map good for collision avoidance - // but it's not sufficient for navigation, e.g., it cannot - // guide around long penninsula. - // Maybe have high-res map that's very local to Alice and - // build that from low-res map. Don't worry about destination - // being in map. Coarse path can be inheritted from low-res map. - // This should save time generating local high-res map. - - gaia_log::app().info("Destination has changed. Rebuilding area map."); - build_area_map(/area_map); } - - on_update(e: error_correction) - { - // TODO decide what to do next. - // For now, start a new path and keep exploring. - gaia_log::app().info("Error has been updated. Start a new path."); - create_new_path(); - } - - - //////////////////////////////////////////// - // Map updates - - on_update(a: area_map) + on_insert(o: observations) { - // Local map was updated. Now rebuild working map. - gaia_log::app().info("Area map built. Rebuilding local map."); - build_local_map(/local_map); + // TODO Consult working map and update path toward destination. } - - on_update(m: local_map) + on_insert(o: observations) { - // Local map was updated. Now rebuild working map. - gaia_log::app().info("Local map built. Rebuilding working map."); - build_working_map(/working_map); + // TODO Perform spatial query in area of observation and see if + // there are other observations to build edges with. } - on_update(w: working_map) - { - // Working map was updated. Based on contents of map, move - // toward destination. - // Alice control drops down to procedural code here, with - // direction setting, forward motion, and distance travelled - // managed at procedural level. When Alice has moved far enough - // forward, an observation record will be made. - gaia_log::app().info("Working map built. Moving toward destination."); - move_toward_destination(); - } - //////////////////////////////////////////// // Async events From 96edd7a6f6e2480b3cc9f397324eb998d87c21d9 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Thu, 31 Mar 2022 13:55:13 -0700 Subject: [PATCH 04/30] Draft of rules/ddl/API --- slam_sim/gaia/slam.ddl | 4 + slam_sim/gaia/slam.ruleset | 93 +++++++++++----------- slam_sim/include/constants.hpp | 27 +++++++ slam_sim/include/slam_sim.hpp | 141 ++++++++++++--------------------- 4 files changed, 127 insertions(+), 138 deletions(-) create mode 100644 slam_sim/include/constants.hpp diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 641a5ab..097417c 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -62,6 +62,8 @@ table ego current_graph references graphs where ego.current_graph_id = graphs.id, + timestamp_sec double, + -- Explicitly created references. -- Keep most of ego data in different tables so that updates to -- one field don't risk conflict and txn rollback. @@ -218,6 +220,8 @@ table observations graph references graphs where graphs.id = observations.graph_id, + timestamp_sec double, + ------------------------------ -- Dynamic fields. These can be modified asynchronously. Functions -- and rules that modify them (i.e., that add edges) should be designed diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 96574d9..30c8f89 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -23,31 +23,33 @@ #include "slam_sim.hpp" -//using slam_sim::select_destination; -//using slam_sim::select_landmark_destination; +using slam_sim::optimization_required; +using slam_sim::optimize_graph; -// Brings Alice to a halt. Creates new observation at present position. -using slam_sim::full_stop; +using slam_sim::need_to_extend_map; +using slam_sim::build_area_map; +using slam_sim::build_working_map; -// Instructs Alice to beging trek to destination. Motion control is -// managed at lower level, using direction data determined in rules. +using slam_sim::select_new_destination; using slam_sim::move_toward_destination; +using slam_sim::full_stop; -//using slam_sim::create_new_path; -//using slam_sim::create_observation; -//using slam_sim::init_path; -// -//using slam_sim::calc_path_error; -//using slam_sim::build_area_map; -//using slam_sim::build_local_map; -//using slam_sim::build_working_map; -// -//using gaia::slam::pending_destination_t; -//using gaia::slam::estimated_position_t; -//using gaia::slam::destination_t; -//using gaia::slam::paths_t; -// -//using gaia::slam::paths_writer; + +/*********************************************************************** +Rules + +map_ruleset serial_group + on_insert(edges) If optimization required, do so and rebuild maps. + on_change(destination) Rebuild working map, move to destination. + on_insert(observation) Extend map if needed, rebuild working map. + +ruleset + on_insert(observation) Spatial query to link to other nodes. + on_insert(observation) Determine if it's time to change destination. + on_insert(collision_event) Full stop and create observation. + + +***********************************************************************/ // Wrap error correction and map creation in a serial group as these are @@ -91,47 +93,43 @@ ruleset map_ruleset : serial_group() { build_area_map(/area_map, /observed_area); } - build_working_map(g, /area_map); + build_working_map(/destination, /area_map); } } -ruleset observation_ruleset +ruleset runtime_ruleset { - //////////////////////////////////////////// - // Creating observations - - // When our position is updated a new observation will be created. - // Note that this is a relatively slow operation so hopefully there's - // no transaction conflict. - on_update(ep: estimated_position) - { - gaia_log::app().info("Estimated position changed to {},{}", ep.x_meters, - ep.y_meters); + // Observations are created at a lower infrastructure level. The + // decision to do so is based on the following assumptions about + // when an observation is made: + // 1) Observations (nodes) are generated at locations that provide + // non-redundant information about the environment, such as range + // data that covers previously unexplored territory or being + // near salient landmark to form closures with other nearby + // observations. + // 2) Sensor data is read at a rate higher than observations are + // created. + // 3) The time required to evaluate whether or not to make an + // observation may be longer than the interval between successive + // packets of sensor data. + // This latter assumption means that one can't directly use a rule + // triggering of sensor data to build observations as these must + // run in parallel, risking violation of (1). - // Determine if it's time to build another observation. - if (should_build_new_observation()) - { - create_observation(/ego.current_path->paths); - } - } on_insert(o: observations) { - // TODO Consult working map and update path toward destination. + // TODO Perform spatial query in area of observation and see if + // there are other observations to build edges with. } on_insert(o: observations) { - // TODO Perform spatial query in area of observation and see if - // there are other observations to build edges with. + // Determine if it's time to change destinations. + select_new_destination(); } - - - //////////////////////////////////////////// - // Async events - on_insert(c: collision_event) { // A collision was detected, or the range sensor detected something @@ -141,3 +139,4 @@ ruleset observation_ruleset full_stop(); } } + diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp new file mode 100644 index 0000000..a8004f6 --- /dev/null +++ b/slam_sim/include/constants.hpp @@ -0,0 +1,27 @@ +#pragma once + +namespace slam_sim +{ + +// Degree/Radian conversions. +constexpr float c_rad_to_deg = 180.0 / M_PI; +constexpr float c_deg_to_rad = M_PI / 180.0; + +// Width (height) of grids in occupancy grid. +// Area map is to provide coarse guidance for route planning. +constexpr float c_area_map_node_width_meters = 0.25; +// Working map is local and provides finer local details for collision +// avoidance. +constexpr float c_working_map_node_width_meters = 0.1; +// Output map is what's exported for observation. +constexpr float c_output_map_node_width_meters = 0.1; + +// Max distance range sensors can get range data. +constexpr float c_range_sensor_max_meters = 4.0; +// Width of range sensor sweep in front of bot. +constexpr float c_range_sensor_sweep_degs = 90.0; +constexpr int32_t c_num_range_radials = 45; + + +} // namespace slam_sim + diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 5971470..5cd45fa 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -32,121 +32,80 @@ namespace slam_sim // Flag to indicate app should exit. Normally this is 0. When it's time // to quit it's set to 1. extern int32_t g_quit; -constexpr int32_t EXIT_AFTER_X_PATHS = 3; -// Low-res map Alice has created of the surroundings. -extern occupancy_grid_t* g_area_map; -constexpr float AREA_MAP_NODE_WIDTH_METERS = 0.25; -constexpr float LOCAL_MAP_NODE_WIDTH_METERS = AREA_MAP_NODE_WIDTH_METERS / 4.0; +//////////////////////////////////////////////// +// Rules API +// This is the interface that is expected to invoked by rules +// (i.e., in the ruleset file). A transaction is expected to +// already be open when these are called. -// If world is AxB, have default size be (A+2)x(B+2) -constexpr float DEFAULT_AREA_MAP_WIDTH_METERS = 16.0; -constexpr float DEFAULT_AREA_MAP_HEIGHT_METERS = 12.0; +// Determines if it's time to perform a graph optimization. +bool optimization_required(); +// Stub function to graph optimization. Contents of function show how +// to iterate through a graph's nodes ('observations') and edges. +void optimize_graph(gaia::slam::graphs_t&); -// Flags to indicate state of Alice's movement. This information is -// stored in the present path. -// 'done' indicates that path has been completed. -// 'starting' indicates that path is just started. -// 'active' indicates that Alice is moving away from a landmark and -// toward a destination. -// 'find_landmark' indicaates that exploration is over and Alice is -// looking for a landmark to get a position fix. -constexpr int32_t PATH_STATE_STARTING = 1; -constexpr int32_t PATH_STATE_ACTIVE = 2; -constexpr int32_t PATH_STATE_FIND_LANDMARK = 4; -constexpr int32_t PATH_STATE_DONE = 8; -// How near to the destination we have to be to say "close enough". -constexpr double DESTINATION_RADIUS_METERS = 0.5; +// Checks to see if known world has extended beyong map boundaries so much +// that it's time to rebuild the area map. +bool need_to_extend_map(); -// -constexpr double INTER_OBSERVATION_DIST_METERS = 0.5; +// Generates a low-res map off the known world and generates coarse path +// info to the destination. +// Stores output 'area_map' record. +void build_area_map(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); -// How close to a landmark we need to be to be able to use it as a -// position fix. -constexpr double LANDMARK_DISTANCE_METERS = 1.0; +// Generates a higher-res map of the local area for local route planning +// and collision avoidance, and uses the area map for the map's path +// boundary conditions. +// Stores output 'working_map' record. +void build_working_map(gaia::slam::working_map_t&); -//////////////////////////////////////////////// -// Initialization +// Checks to see if it's time to select a new destination. If so, the +// destination record is updated and the function returns 'true'. Otherwise +// returns 'false'. +bool select_new_destination(); -// Params are Alice's starting point. -void seed_database(double initial_x_meters, double iniital_y_meters); +// Instructs hardware layer to move toward destination. +void move_toward_destination(gaia::slam::working_map_t&); +// Instructs bot to stop. An observation will be taken here. +void full_stop(); -//////////////////////////////////////////////// -// Going places -// Creates a record in the pending_destination table, or updates -// the record that is already there. -void request_new_destination(double x_meters, double y_meters); -// Select position to go to. Default beahvior is to explore environment. -void select_destination(); +//////////////////////////////////////////////// +// Support API -// Sets path.state to FIND_LANDMARK and selects a landmark to go to. -void select_landmark_destination(); +// Creates a record in the observation table using the supplied sensor +// data +void create_observation(const slam_sim::sensor_data_t& data); -// Called in unusual situations, such as if Alice detects a collision or -// senses that one is imminent. Brings the bot to a halt and performs -// no further actions. If called when bot is already stopped then -// request is ignored. -void full_stop(); +// Do a sensor sweep from at the stated position. This would normally be +// pushed up from the sensor/hardware layer, but polling for it makes +// the simulation more straightforward. +void generate_sensor_data(double pos_x_meters, double pos_y_meters, + slam_sim::sensor_data_t& data); -// Moves Alice toward one step toward the destination. The working map -// is consulted to to determine a path to follow. Alice moves forward -// a fixed straight-line distance and then stops and creates an observation. -void move_toward_destination(); +// Generates and exports a map to disk. Filename will be based on ego's +// timestamp. +void export_map_to_disk(); -// Start a new path. This creates an initial observation and links it -// to a path. -void create_new_path(); +// External request to move to a specific location. +void request_destination(double x_meters, double y_meters); -// Initialize a new path and assign it its first observation. -void init_path(const gaia::slam::observations_t&); - -// Loads a .json file that describes the environment. -void load_world_map(const char*); //////////////////////////////////////////////// -// Seeing people - -// Performs a full sensor sweep of the environment and creates a record -// in the 'observations' table. -// First brings Alice to a halt if not already stopped. -// Creates observations record, updates estimated_position record. -void create_observation(gaia::slam::paths_t&); - -// Do a sensor sweep from at the stated position. -void perform_sensor_sweep(double pos_x_meters, double pos_y_meters, - utils::sensor_data_t& data); +// Initialization +// Params are Alice's starting point. +void seed_database(double initial_x_meters, double iniital_y_meters); -//////////////////////////////////////////////// -// Mapping - -// Given position fixes at the start and end point of the path, estimate -// DR error and update DR error estimate. -// Updates error_correction record. -void calc_path_error(gaia::slam::paths_t& path); - -// Generates a low-res map off the area with path information to destination. -// Stores in 'area_map' record. -void build_area_map(gaia::slam::area_map_t&); - -// Generates a high-res map of the area, based on previously acquired -// and calibrated data. -// Stores output in 'local_map' record. -void build_local_map(gaia::slam::local_map_t&); - -// Generates a high-res map of the area using recently acquired sensor data, -// aligned as best as possible. Builds a path map to destination, using -// area map to provide boundary conditions, and obstacle information from -// local map to supplement recently acquired sensor data. -// Updates working_map record. -void build_working_map(gaia::slam::working_map_t&); +// Loads a .json file that describes the environment. +void load_world_map(const char*); } // namespace slam_sim From fe0b88883b7516db71b84803aeff74232cc594ff Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Thu, 31 Mar 2022 16:39:23 -0700 Subject: [PATCH 05/30] Migrating trislam changes to slam_sim --- slam_sim/gaia/slam.ddl | 21 +++- slam_sim/gaia/slam.ruleset | 16 +-- slam_sim/include/occupancy.hpp | 42 +------ slam_sim/include/slam_sim.hpp | 3 +- slam_sim/src/Makefile | 2 +- slam_sim/src/analyze.cpp | 109 +++++------------- slam_sim/src/globals.cpp | 8 -- slam_sim/src/{slam_sim.cpp => main.cpp} | 69 +++++------- slam_sim/src/occupancy.cpp | 139 ++++++++--------------- slam_sim/src/rule_api.cpp | 143 ++++++++++++++++++++++++ 10 files changed, 273 insertions(+), 279 deletions(-) rename slam_sim/src/{slam_sim.cpp => main.cpp} (70%) create mode 100644 slam_sim/src/rule_api.cpp diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 097417c..08984f5 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -34,6 +34,7 @@ database slam; -- area_map Map of the known world. -- working_map Version of world map with recent sensor overlay. -- destination Location that bot is moving to. +-- observed_area Bounds of observed world. -- -- Event tables: -- pending_destination Destination requested externally. @@ -62,7 +63,8 @@ table ego current_graph references graphs where ego.current_graph_id = graphs.id, - timestamp_sec double, + -- Most recent timestmap is stored in most recent observation + -- (referenced below). -- Explicitly created references. -- Keep most of ego data in different tables so that updates to @@ -74,12 +76,10 @@ table ego latest_observation references latest_observation, -- Position oriented. - world references observed_area destination references destination, world references observed_area, -- Map oriented. - world references observed_area low_res_map references area_map, working_map references working_map ) @@ -140,6 +140,21 @@ table destination ) +-- Bounds of known world size. When maps are generated, they can use these +-- values for map bounds. +table observed_area +( + -- Bounding polygon + -- Uses world coordinates, with increasing X,Y being rightward/upward. + left_meters float, + right_meters float, + top_meters float, + bottom_meters float, + + ego references ego +) + + -- Represents the most recently created observation. -- In the context of evaluating the data model for possible transaction -- conflicts, If multiple observations are not created simultaneously diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 30c8f89..9328aae 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -60,12 +60,12 @@ ruleset map_ruleset : serial_group() // We don't want multiple edges to be created at similar points in time // and both realize that it's time to optimize the map, and do so // together, so keep the graph optimization check in a serial group. - on_insert(e: edges) + on_insert(edges) { // New edge created. See if it's time to run new graph optimization. if (optimization_required()) { - optimize_graph(e.dest->observations.graph->graphs); + optimize_graph(edges.dest->observations.graph->graphs); // Whenever new error-correction data is available, regenerate // maps. This can be triggered in a separate rule or simply @@ -77,15 +77,15 @@ ruleset map_ruleset : serial_group() // Destination changed. Update working map and start moving toward it. - on_change(d: destination) + on_change(destination) { - build_working_map(d, /area_map); + build_working_map(destination, /area_map); move_toward_destination(/working_map); } // For each observation, update working map using new sensor data. - on_insert(o: observations) + on_insert(observations) { // If working map will go beyond border of area map, rebuild // area map. @@ -118,19 +118,19 @@ ruleset runtime_ruleset // run in parallel, risking violation of (1). - on_insert(o: observations) + on_insert(observations) { // TODO Perform spatial query in area of observation and see if // there are other observations to build edges with. } - on_insert(o: observations) + on_insert(observations) { // Determine if it's time to change destinations. select_new_destination(); } - on_insert(c: collision_event) + on_insert(collision_event) { // A collision was detected, or the range sensor detected something // closer than it's supposed to be. Initiate a stop, which will diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index dda2045..9549bfc 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -25,17 +25,15 @@ As elsewhere in the code, positive coordinates are to the up and right. #include "gaia_slam.h" #include "sensor_data.hpp" -#include "landmark_description.hpp" namespace slam_sim { // Characteristics of a node in the map, including distance from this node // to a/the destination. -// Occupied: how many times the node has been travelled through. +// Occupied: how many times the node has been traveled through. // Observed: how many times the node has been seen. // Boundary: how many times the node had an obstruction detection in it. -// Landmark: how many times the node had landmark reported in it. struct map_node_t { // Index of node that's one-closer to destination (-1 for no parent) @@ -47,7 +45,6 @@ struct map_node_t float occupied; float observed; float boundary; - float landmarks; // Cost to traverse this node. This is computed as a function of the // node's characteristics. @@ -67,7 +64,6 @@ struct map_node_flags_t uint8_t occupied; uint8_t observed; uint8_t boundary; - uint8_t landmark; void clear(); }; @@ -106,8 +102,8 @@ class occupancy_grid_t public: // Map center starts at 0,0 so map bounds are at +/- width/2 and // +/- height/2. - occupancy_grid_t(float node_width_meters, float width_meters, - float height_meters); + occupancy_grid_t(float node_width_meters, world_coordinate_t top_left, + world_coordinate_t bottom_right); ~occupancy_grid_t(); // Initialization @@ -118,8 +114,6 @@ class occupancy_grid_t // based on a larger lower resolution area map. //void embed(const occupancy_grid_t& surrounding); - //void resize(float width_meters, float height_meters); - // Returns a reference to tne map node at the specified location. map_node_t& get_node(float x_meters, float y_meters); map_node_flags_t& get_node_flags(float x_meters, float y_meters); @@ -131,8 +125,7 @@ class occupancy_grid_t protected: uint32_t get_node_index(float pos_x_meters, float pos_y_meters); - void apply_landmarks(const gaia::slam::observations_t&); - void apply_radial(uint32_t radial, float range_meters, + void apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters); void apply_flags(); @@ -141,8 +134,7 @@ class occupancy_grid_t map_size_t m_map_size; // Positive coordiantes are rightward and upward. - world_coordinate_t m_top_left; - world_coordinate_t m_bottom_right; + world_coordinate_t m_bottom_left; std::vector m_grid; std::vector m_grid_flags; @@ -150,29 +142,5 @@ class occupancy_grid_t world_coordinate_t m_destination; }; - -////////////////////////////////////////////////////////////////////////// -//// API -// -//// Usage: initialize the map, apply sensor data to it, add destinations, -//// and then compile. Map nodes will have vectors to move toward -//// destination. -// -//void reset_map(occupancy_grid_t& map); -//// Initializes inner map using outer map to set boundary conditions. This -//// is meant to be used to build a higher resolution local map to use -//// based on a larger lower resolution area map. -//void embed_map(const occupancy_grid_t& outer, occupancy_grid_t& inner); -// -//// Apply sensor data to it. -//void apply_sensor_data_to_map(occupancy_grid_t& map, sensor_data_t& data); -// -//// Add destination to the map. -//void add_destination(occupancy_grid_t& map, double x_meters, y_meters); -// -//// Build distances to destination(s) and directional vectors. -//void compile_map(occupancy_grid_t& map); -// - } // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 5cd45fa..66250c1 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -76,7 +76,6 @@ void move_toward_destination(gaia::slam::working_map_t&); void full_stop(); - //////////////////////////////////////////////// // Support API @@ -92,7 +91,7 @@ void generate_sensor_data(double pos_x_meters, double pos_y_meters, // Generates and exports a map to disk. Filename will be based on ego's // timestamp. -void export_map_to_disk(); +void export_map_to_file(); // External request to move to a specific location. void request_destination(double x_meters, double y_meters); diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 5fbed7b..bace2e1 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -17,4 +17,4 @@ refresh_db: gaiac ../gaia/slam.ddl --db-name slam -g -o /tmp/slam clean: - rm *.pgm + rm -f *.pgm diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index 5896f28..5b2854e 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -8,17 +8,9 @@ //////////////////////////////////////////////////////////////////////// // -// A very simple environment simulation interface. -// // This file calculates the sensor view of the environment. The // environment is made up of a 2D map (flatland) that has walls and -// explicit landmarks. In SLAM, landmarks will typically be calculated -// based on aligning salient features between different observations. -// In this case, such alignment is assumed to have been done, as the -// point of the example here is what to do when you find a landmark, -// not feature extraction and mapping. A more intuitive way to think -// about landmarks in this example, as its implemented, is that -// they're QR codes on a wall. +// objects (e.g., 'tables'). // //////////////////////////////////////////////////////////////////////// @@ -32,6 +24,7 @@ #include "json.hpp" #include "line_segment.hpp" #include "sensor_data.hpp" +#include "slam_sim.hpp" namespace slam_sim { @@ -40,15 +33,8 @@ using std::vector; using std::string; using nlohmann::json; -using utils::line_segment_t; -using utils::landmark_description_t; -using utils::sensor_data_t; - -constexpr int32_t NUM_RANGE_RADIALS = 180; - // Line segments describing outline of all objects in the world. vector g_world_lines; -vector g_landmarks; // Parse json map data and copy to vectors describing the world. Assume @@ -64,26 +50,14 @@ static void set_map(const char* map) for (uint32_t j=0; j= 360.0 ? theta_degs - 360.0 : theta_degs; + theta_degs = theta_degs < 0.0 ? theta_degs + 360.0 : theta_degs; + // Measure distance on this radial + float min_meters = -1.0; int32_t line_num = -1; for (uint32_t i=0; i 0.0) { if ((min_meters < 0.0) || (dist_meters < min_meters)) @@ -140,49 +120,14 @@ printf("RANGES from %.3f,%.3f\n", x_meters, y_meters); } } } -printf(" %3d %8.2f %6.2f %4d\n", n, theta_degs, min_meters, line_num); - if (min_meters > utils::RANGE_SENSOR_MAX_METERS) + if (min_meters > c_range_sensor_max_meters) { min_meters = -1.0; } data.range_meters.push_back(min_meters); + data.bearing_degs.push_back(theta_degs); } } - -static void calculate_landmarks(double x_meters, double y_meters, - sensor_data_t& data) -{ - data.landmarks_visible.clear(); - // Calculate range and bearing of each landmark. - // Landmarks must be arranged so that if they're w/in visual range then - // there's only one way to see them (i.e., not through a wall or on - // the back side of one). - for (const landmark_description_t& landmark: g_landmarks) - { - double dx = x_meters - landmark.x_meters; - double dy = y_meters - landmark.y_meters; - double range_meters = sqrt(dx*dx + dy*dy); - if (range_meters <= utils::LANDMARK_VISIBILITY_METERS) - { -//printf("Close to landmark %.3f\n", range_meters); - data.landmarks_visible.push_back(landmark); - } - } -} - - -void perform_sensor_sweep(double x_meters, double y_meters, sensor_data_t& data) -{ - gaia_log::app().info("Sensor sweep at {},{}", x_meters, y_meters); - // Perform 360-degree sensor sweep. - calculate_ranges(x_meters, y_meters, data); - calculate_landmarks(x_meters, y_meters, data); -} - - -// Build sensor view -//////////////////////////////////////////////////////////////////////// - - } // namespace slam_sim + diff --git a/slam_sim/src/globals.cpp b/slam_sim/src/globals.cpp index b757358..66ab47f 100644 --- a/slam_sim/src/globals.cpp +++ b/slam_sim/src/globals.cpp @@ -8,17 +8,9 @@ #include -#include "occupancy.hpp" - namespace slam_sim { int32_t g_quit = 0; -// Area map is not presently protectected against being accessed from -// multiple threads simultaneously. Simultaneous access shouldn't occur -// due to the way rules are constructed but protection should still be -// added. TODO -occupancy_grid_t* g_area_map; - } diff --git a/slam_sim/src/slam_sim.cpp b/slam_sim/src/main.cpp similarity index 70% rename from slam_sim/src/slam_sim.cpp rename to slam_sim/src/main.cpp index fc47424..5253c46 100644 --- a/slam_sim/src/slam_sim.cpp +++ b/slam_sim/src/main.cpp @@ -22,17 +22,14 @@ #include "slam_sim.hpp" -using namespace gaia::slam; - -using std::this_thread::sleep_for; namespace slam_sim { +using std::this_thread::sleep_for; + constexpr uint32_t c_rule_wait_millis = 100; -static float g_initial_x_meters = -1.0; -static float g_initial_y_meters = -1.0; /** * Wait for simulation to complete. @@ -48,35 +45,37 @@ void main_loop() } -template -void clear_table() +template +void remove_all_rows() { - for (auto obj_it = T_type::list().begin(); obj_it != T_type::list().end(); ) + const bool force_disconnect_of_related_rows = true; + for (auto obj_it = T_object::list().begin(); + obj_it != T_object::list().end();) { - auto current_obj_it = obj_it++; - current_obj_it->delete_row(); + auto this_it = obj_it++; + this_it->delete_row(force_disconnect_of_related_rows); } } void clear_data() { - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); - clear_table(); + gaia::db::begin_transaction(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + remove_all_rows(); + gaia::db::commit_transaction(); } @@ -152,15 +151,6 @@ void init_sim() // Seeding function manages its own transaction. gaia_log::app().info("Seeding the database..."); seed_database(g_initial_x_meters, g_initial_y_meters); - - // Create map(s). - g_area_map = new occupancy_grid_t(AREA_MAP_NODE_WIDTH_METERS, - DEFAULT_AREA_MAP_WIDTH_METERS, DEFAULT_AREA_MAP_HEIGHT_METERS); - - gaia_log::app().info("Creating initial path..."); - gaia::db::begin_transaction(); - create_new_path(); - gaia::db::commit_transaction(); } } // namespace slam_sim; @@ -172,14 +162,7 @@ int main(int argc, char** argv) slam_sim::parse_command_line(argc, argv); - // We explicitly handle the transactions with begin_transaction() - // and commit_transaction() to trigger the rules. - gaia::db::begin_transaction(); - slam_sim::clear_data(); - gaia::db::commit_transaction(); - - gaia_log::app().info("Starting SLAM simulation..."); - + slam_sim::clean_db(); slam_sim::init_sim(); slam_sim::main_loop(); diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 1b53dc0..9fe3714 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -5,6 +5,13 @@ // license that can be found in the LICENSE.txt file // or at https://opensource.org/licenses/MIT. //////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////// +// +// Logic for making/updating binary occupancy grid. +// +//////////////////////////////////////////////////////////////////////// + #include #include @@ -22,62 +29,30 @@ using std::vector; using std::string; using std::cerr; -using utils::sensor_data_t; -using utils::landmark_description_t; -using utils::D2R; - -using gaia::slam::landmark_sightings_t; using gaia::slam::observations_t; +using gaia::slam::positions_t; +using gaia::slam::range_data_t; -//////////////////////////////////////////////////////////////////////// -// Lookup table for caching sin/cos values - -// Instead of computing sine and cosine all the time, precompute these -// values for each range radial and cache it. -struct sin_cos_t { - float s, c; -}; - -static sin_cos_t* s_sincos_lut = NULL; -static uint32_t s_sincos_lut_len = 0; - - -static void check_sincos_lut(uint32_t num_radials) -{ - if (s_sincos_lut_len == 0) - { - s_sincos_lut = new sin_cos_t[num_radials]; - // LUT isn't initialized. Do so now. - double step_degs = 360.0 / num_radials; - for (uint32_t i=0; ioccupied = 0.0f; this->observed = 0.0f; this->boundary = 0.0f; - this->landmarks = 0.0f; this->traversal_cost = 0.0f; this->distance = 0.0f; @@ -109,7 +87,6 @@ void map_node_flags_t::clear() this->occupied = 0; this->observed = 0; this->boundary = 0; - this->landmark = 0; } @@ -133,15 +110,17 @@ void occupancy_grid_t::clear() // Returns index of map node at specified location. Location uses uint32_t occupancy_grid_t::get_node_index(float x_meters, float y_meters) { - float left_inset_meters = x_meters - m_top_left.x_meters; - uint32_t x_idx = (uint32_t) floor(left_inset_meters / m_map_size.x_meters); + // x index. + float left_inset_meters = x_meters - m_bottom_left.x_meters;; + uint32_t x_idx = (uint32_t) floor(left_inset_meters / m_node_size_meters); if (x_idx >= m_grid_size.cols) { x_idx = m_grid_size.cols - 1; } - float bottom_inset_meters = y_meters - m_bottom_right.y_meters; + // y index. + float bottom_inset_meters = y_meters - m_bottom_left.y_meters;; uint32_t y_idx = - (uint32_t) floor(bottom_inset_meters / m_map_size.y_meters); + (uint32_t) floor(bottom_inset_meters / m_node_size_meters); if (y_idx >= m_grid_size.rows) { y_idx = m_grid_size.rows - 1; @@ -171,19 +150,19 @@ map_node_flags_t& occupancy_grid_t::get_node_flags( // Updates occupancy, oberved and boundary flags in map from // observations on this radial. // Steps through radial and estimates what grid squares it crosses. -void occupancy_grid_t::apply_radial(uint32_t radial, float range_meters, +void occupancy_grid_t::apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters) { // Set occupancy for this grid square. map_node_flags_t& home_flags = get_node_flags(pos_x_meters, pos_y_meters); +//printf("OCCUPANCY position %.2f,%.2f\n", pos_x_meters, pos_y_meters); home_flags.occupied = 1; - // Set observed and boundary flags. // Measured distance on radial. double dist_meters = range_meters < 0.0 - ? utils::RANGE_SENSOR_MAX_METERS : range_meters; - - const sin_cos_t& sc = s_sincos_lut[radial]; + ? c_range_sensor_max_meters : range_meters; + float s, c; + sincosf(c_deg_to_rad * radial_degs, &s, &c); // Number of points on radial to examine. Sample at a fraction of // the grid size. uint32_t num_steps = (uint32_t) @@ -194,43 +173,20 @@ void occupancy_grid_t::apply_radial(uint32_t radial, float range_meters, // Get position in world map, adjusting relative range data by // bot's absolute position. float dist = (float) i * step_size_meters; - float x_pos = dist * sc.s - pos_x_meters; - float y_pos = dist * sc.c - pos_y_meters; + float x_pos = dist * s + pos_x_meters; + float y_pos = dist * c + pos_y_meters; map_node_flags_t& flags = get_node_flags(x_pos, y_pos); flags.observed = 1; // If this is the end of the radial and a range was detected, // mark the boundary flag. if ((i == num_steps) && (range_meters > 0.0)) { -printf(" Boundary on %d at %.2f,%.2f (%.2f, %.2f, %.2f)\n", radial, x_pos, y_pos, pos_x_meters, pos_y_meters, dist); flags.boundary = 1; } } } -// Updates landmark flags. -void occupancy_grid_t::apply_landmarks(const observations_t& obs) -{ - float pos_x_meters = obs.pos_x_meters(); - float pos_y_meters = obs.pos_y_meters(); - for (const landmark_sightings_t& ls: obs.landmark_sightings()) - { - int32_t radial = (int32_t) floor(obs.num_radials() - * ls.bearing_degs() / 360.0); - assert(radial >= 0); - assert(radial < obs.num_radials()); - const sin_cos_t& sc = s_sincos_lut[radial]; - float dx_meters = ls.range_meters() * sc.s; - float dy_meters = ls.range_meters() * sc.c; - - map_node_flags_t& flags = get_node_flags(pos_x_meters + dx_meters, - pos_y_meters + dy_meters); - flags.landmark = 1; - } -} - - // Increments map_node_t values from contents of map_node_flags_t. void occupancy_grid_t::apply_flags() { @@ -238,27 +194,23 @@ void occupancy_grid_t::apply_flags() { map_node_flags_t& flags = m_grid_flags[i]; map_node_t& node = m_grid[i]; - node.occupied += flags.occupied; node.observed += flags.observed; node.boundary += flags.boundary; - node.landmarks += flags.landmark; } } void occupancy_grid_t::apply_sensor_data(const observations_t& obs) { - check_sincos_lut(obs.num_radials()); -printf("Applying sensor data at %.2f,%.2f (%d)\n", obs.pos_x_meters(), obs.pos_y_meters(), obs.id()); - float pos_x_meters = obs.pos_x_meters(); - float pos_y_meters = obs.pos_y_meters(); - for (int32_t i=0; i 0.0 ? 128 : 0; + g[idx] = node.occupied > 0.0 ? 255 : 0; } } diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp new file mode 100644 index 0000000..26485e8 --- /dev/null +++ b/slam_sim/src/rule_api.cpp @@ -0,0 +1,143 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////// +// +// Primary API for SLAM-related rules. +// +// Maps are generated from sensor data stored in the current graph. +// +//////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "gaia_slam.h" + +#include "occupancy.hpp" +#include "slam_sim.hpp" + +namespace slam_sim +{ + +using std::string; + +using gaia::slam::edges_t; +using gaia::slam::ego_t; +using gaia::slam::error_corrections_t; +using gaia::slam::graphs_t; +using gaia::slam::observed_area_t; +using gaia::slam::observations_t; + + +// Determine if a new graph optimization is necessary. +// In a live example, this function would apply logic to determine if +// enough data has been collected (e.g., new range data, new closures, +// etc.) to justify performing another map optimization. For now, say +// an optiimzation is required every X observations. +bool optimization_required() +{ + static int32_t ctr = 0; + const int32_t MAP_INTERVAL = 10; + if ((++ctr % MAP_INTERVAL) == 0) + { + return true; + } + return false; +} + + +// Optimze the graph. +// In a live example this would iterate through all edges/nodes and +// do some math wizardry to reduce positional error of the nodes. For +// now, iterate though the nodes/edges to demonstrate how that works +// and just print error to the log. +void optimize_graph(graphs_t& graph) +{ + // Map optimization logic goes here. Here are some ways to iterate + // through the data. + gaia_log::app().info("Stub function to optimize graph {}", graph.id()); + // By edges: + for (edges_t& e: edges_t::list()) + { + gaia_log::app().info("Edge connects observations {} and {}", + e.src().id(), e.dest().id()); + } + // By observations: + for (observations_t& o: observations_t::list()) + { + gaia_log::app().info("Obervation {} connects to:", o.id()); + for (edges_t& e: o.in_edges()) + { + gaia_log::app().info(" (in) obervation {}", e.src_id()); + } + for (edges_t& e: o.out_edges()) + { + gaia_log::app().info(" (out) obervation {}", e.dest_id()); + } + } + // Create error correction record. This serves to store error correction + // data, if necessary, as well as trigger a rule event. + static uint32_t next_ec_id = 1; + error_corrections_t::insert_row( + next_ec_id, // id + graph.id() // graph_id + ); + next_ec_id++; +} + + +//////////////////////////////////////////////////////////////////////// +// Map building + +static void write_map_to_file(occupancy_grid_t& map) +{ + static int32_t ctr = 0; + // There are C++ ways to format a number to a string (e.g., + // fmt::format or std::stringstream, but the C approach is + // nice in its simplicity. + char fname[256]; + sprintf(fname, "map_%03d.pgm", ctr++); + gaia_log::app().info("Building map {}", fname); + map.export_as_pnm(fname); +} + + +void build_map() +{ + gaia::db::begin_transaction(); + ego_t ego = *(ego_t::list().begin()); + observed_area_t& area = *(observed_area_t::list().begin()); + build_map(ego.current_graph(), area); + gaia::db::commit_transaction(); +} + + +void build_map(const graphs_t& g, const observed_area_t& bounds) +{ + world_coordinate_t top_left = { + .x_meters = bounds.left_meters(), + .y_meters = bounds.top_meters() + }; + world_coordinate_t bottom_right = { + .x_meters = bounds.right_meters(), + .y_meters = bounds.bottom_meters() + }; + occupancy_grid_t map(c_map_node_width_meters, top_left, bottom_right); + // Iterate through observations in this graph and build a map. + for (const observations_t& o: g.observations()) + { + gaia_log::app().info("Applying sensor data from observation {}", + o.id()); + map.apply_sensor_data(o); + } + write_map_to_file(map); +} + +} // namespace slam_sim + From d0b27ddb67ae05ea1a53d50fe123c2d2adf844ca Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Fri, 1 Apr 2022 08:25:47 -0700 Subject: [PATCH 06/30] Minor tweak to ddl for maps --- slam_sim/gaia/slam.ddl | 36 +++++++++++++++++++++--------------- slam_sim/gaia/slam.ruleset | 5 +++-- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 08984f5..7c88522 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -85,6 +85,21 @@ table ego ) +-- Bounds of known world size. When maps are generated, they can use these +-- values for map bounds. +table observed_area +( + -- Bounding polygon + -- Uses world coordinates, with increasing X,Y being rightward/upward. + left_meters float, + right_meters float, + top_meters float, + bottom_meters float, + + ego references ego +) + + -- Maps would ideally store map content as blobs but max blob size is -- presently too small. Instead, store pointer to current map and -- manage concurrent access manually. @@ -100,6 +115,9 @@ table area_map right_meters float, top_meters float, bottom_meters float, + -- Grid size + n_rows uint32, + n_cols uint32, ego references ego working_map references working_map @@ -118,6 +136,9 @@ table working_map right_meters float, top_meters float, bottom_meters float, + -- Grid size + n_rows uint32, + n_cols uint32, -- References ego references ego, @@ -140,21 +161,6 @@ table destination ) --- Bounds of known world size. When maps are generated, they can use these --- values for map bounds. -table observed_area -( - -- Bounding polygon - -- Uses world coordinates, with increasing X,Y being rightward/upward. - left_meters float, - right_meters float, - top_meters float, - bottom_meters float, - - ego references ego -) - - -- Represents the most recently created observation. -- In the context of evaluating the data model for possible transaction -- conflicts, If multiple observations are not created simultaneously diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 9328aae..cc09c43 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -41,7 +41,7 @@ Rules map_ruleset serial_group on_insert(edges) If optimization required, do so and rebuild maps. on_change(destination) Rebuild working map, move to destination. - on_insert(observation) Extend map if needed, rebuild working map. + on_insert(observation) Extend area map if necessary. ruleset on_insert(observation) Spatial query to link to other nodes. @@ -115,7 +115,8 @@ ruleset runtime_ruleset // packets of sensor data. // This latter assumption means that one can't directly use a rule // triggering of sensor data to build observations as these must - // run in parallel, risking violation of (1). + // run in parallel, risking violation of (1), as other observations + // made during processing will be invisible to the rule's txn. on_insert(observations) From 72d651e6e7ad25c8c58eab72e8c4097c7017a72b Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Mon, 4 Apr 2022 11:26:09 -0700 Subject: [PATCH 07/30] Snapshot save during dev. Revert to 'make' for better incremental builds. --- slam_sim/Makefile | 3 +- slam_sim/gaia/Makefile | 4 + slam_sim/gaia/slam.ddl | 24 +- slam_sim/gaia/slam.ruleset | 8 +- slam_sim/include/constants.hpp | 9 +- slam_sim/include/landmark_description.hpp | 34 -- slam_sim/include/line_segment.hpp | 4 +- slam_sim/include/occupancy.hpp | 23 +- slam_sim/include/sensor_data.hpp | 18 +- slam_sim/include/slam_sim.hpp | 5 +- slam_sim/src/Makefile | 36 +- slam_sim/src/analyze.cpp | 6 +- slam_sim/src/occupancy.cpp | 72 ++- slam_sim/src/paths.cpp | 559 -------------------- slam_sim/src/rule_api.cpp | 18 +- slam_sim/src/utils/Makefile | 2 +- slam_sim/src/utils/landmark_description.cpp | 38 -- slam_sim/src/utils/line_segment.cpp | 4 +- 18 files changed, 169 insertions(+), 698 deletions(-) delete mode 100644 slam_sim/include/landmark_description.hpp delete mode 100644 slam_sim/src/paths.cpp delete mode 100644 slam_sim/src/utils/landmark_description.cpp diff --git a/slam_sim/Makefile b/slam_sim/Makefile index b0a9642..9344fbf 100644 --- a/slam_sim/Makefile +++ b/slam_sim/Makefile @@ -1,6 +1,7 @@ all: - mkdir -p build && cd build && cmake .. && make -j 4 + #mkdir -p build && cd build && cmake .. && make -j 4 + cd src && make run: cd src && make run diff --git a/slam_sim/gaia/Makefile b/slam_sim/gaia/Makefile index 60b7256..d892f60 100644 --- a/slam_sim/gaia/Makefile +++ b/slam_sim/gaia/Makefile @@ -17,6 +17,10 @@ OBJS = slam_rules.o slam/gaia_slam.o all: gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME) gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) + +refresh: clean all + +clean: rm -f *.o $(RULES_CPP) rm -rf $(DB_NAME) diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 7c88522..6a7740c 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -105,9 +105,10 @@ table observed_area -- manage concurrent access manually. table area_map ( - // Reference to in-memory blob that stores 2D map. - // Blob ID changes on each map update. - uint32_t blob_id; + -- Reference to in-memory blob that stores 2D map. + -- Blob ID changes on each map update. + -- Area map must be seeded with a different blob_id than working map. + blob_id uint32, -- Bounding polygon -- Uses world coordinates, with increasing X,Y being rightward/upward. @@ -116,19 +117,20 @@ table area_map top_meters float, bottom_meters float, -- Grid size - n_rows uint32, - n_cols uint32, + num_rows uint32, + num_cols uint32, - ego references ego + ego references ego, working_map references working_map ) table working_map ( - // Reference to in-memory blob that stores 2D map. - // Blob ID changes on each map update. - uint32_t blob_id; + -- Reference to in-memory blob that stores 2D map. + -- Blob ID changes on each map update. + -- Working map must be seeded with a different blob_id than area map. + blob_id uint32, -- Bounding polygon -- Uses world coordinates, with increasing X,Y being rightward/upward @@ -137,8 +139,8 @@ table working_map top_meters float, bottom_meters float, -- Grid size - n_rows uint32, - n_cols uint32, + num_rows uint32, + num_cols uint32, -- References ego references ego, diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index cc09c43..f9db2a8 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -71,7 +71,7 @@ ruleset map_ruleset : serial_group() // maps. This can be triggered in a separate rule or simply // done here, so do it here. build_area_map(/area_map, /observed_area); - build_working_map(/destination, /area_map); + build_working_map(/destination, /area_map, /working_map); } } @@ -79,7 +79,7 @@ ruleset map_ruleset : serial_group() // Destination changed. Update working map and start moving toward it. on_change(destination) { - build_working_map(destination, /area_map); + build_working_map(/destination, /area_map, /working_map); move_toward_destination(/working_map); } @@ -89,11 +89,11 @@ ruleset map_ruleset : serial_group() { // If working map will go beyond border of area map, rebuild // area map. - if (need_to_extend_area_map()) + if (need_to_extend_map()) { build_area_map(/area_map, /observed_area); } - build_working_map(/destination, /area_map); + build_working_map(/destination, /area_map, /working_map); } } diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index a8004f6..5bd2b5e 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -8,13 +8,7 @@ constexpr float c_rad_to_deg = 180.0 / M_PI; constexpr float c_deg_to_rad = M_PI / 180.0; // Width (height) of grids in occupancy grid. -// Area map is to provide coarse guidance for route planning. -constexpr float c_area_map_node_width_meters = 0.25; -// Working map is local and provides finer local details for collision -// avoidance. -constexpr float c_working_map_node_width_meters = 0.1; -// Output map is what's exported for observation. -constexpr float c_output_map_node_width_meters = 0.1; +constexpr float c_map_node_width_meters = 0.1; // Max distance range sensors can get range data. constexpr float c_range_sensor_max_meters = 4.0; @@ -22,6 +16,5 @@ constexpr float c_range_sensor_max_meters = 4.0; constexpr float c_range_sensor_sweep_degs = 90.0; constexpr int32_t c_num_range_radials = 45; - } // namespace slam_sim diff --git a/slam_sim/include/landmark_description.hpp b/slam_sim/include/landmark_description.hpp deleted file mode 100644 index 5270992..0000000 --- a/slam_sim/include/landmark_description.hpp +++ /dev/null @@ -1,34 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////////// -// -// Summary description of a landmark. -// -//////////////////////////////////////////////////////////////////////// - -#pragma once - -#include - -namespace utils -{ - -struct landmark_description_t -{ - landmark_description_t(); - landmark_description_t(std::string name, int32_t id, - double x_meters, double y_meters); - - std::string name; - int32_t id; - double x_meters; - double y_meters; -}; - -} // namespace utils diff --git a/slam_sim/include/line_segment.hpp b/slam_sim/include/line_segment.hpp index 2458c59..db54092 100644 --- a/slam_sim/include/line_segment.hpp +++ b/slam_sim/include/line_segment.hpp @@ -16,7 +16,7 @@ #pragma once -namespace utils +namespace slam_sim { constexpr double R2D = 180.0 / M_PI; @@ -49,4 +49,4 @@ class line_segment_t // Remaps degrees to be on [0,360) double unwrap_compass(double theta_degs); -} // namespace utils +} // namespace slam_sim diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 9549bfc..34593d8 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -100,10 +100,10 @@ struct map_size_t class occupancy_grid_t { public: - // Map center starts at 0,0 so map bounds are at +/- width/2 and - // +/- height/2. occupancy_grid_t(float node_width_meters, world_coordinate_t top_left, world_coordinate_t bottom_right); + occupancy_grid_t(gaia::slam::area_map_t&); + occupancy_grid_t(gaia::slam::working_map_t&); ~occupancy_grid_t(); // Initialization @@ -129,15 +129,24 @@ class occupancy_grid_t float pos_x_meters, float pos_y_meters); void apply_flags(); + // This will be fixed for a given map resolution. float m_node_size_meters; - grid_size_t m_grid_size; + // These fields are data dependent. A copy of them is stored in the DB + // alongside a pointer to the memory blob storing the grid itself. + grid_size_t m_grid_size; map_size_t m_map_size; - // Positive coordiantes are rightward and upward. world_coordinate_t m_bottom_left; - - std::vector m_grid; - std::vector m_grid_flags; + // Note that positive coordiantes are rightward and upward. + + // Manage array memory directly, so it can be cached in a blob and + // rehydrated as necessary. If ID is <0 then memory is owned by + // this object, if >=0 then memory is owned by blob_cache. + int32_t m_blob_id; + map_node_t* m_grid; + map_node_flags_t* m_grid_flags; + //std::vector m_grid; + //std::vector m_grid_flags; world_coordinate_t m_destination; }; diff --git a/slam_sim/include/sensor_data.hpp b/slam_sim/include/sensor_data.hpp index 193be21..a519c4a 100644 --- a/slam_sim/include/sensor_data.hpp +++ b/slam_sim/include/sensor_data.hpp @@ -16,13 +16,15 @@ #include -#include "landmark_description.hpp" - -namespace utils +namespace slam_sim { -// How close Alice has to be to see the landmark. -constexpr double LANDMARK_VISIBILITY_METERS = 3.0; +struct map_coord_t +{ + float x_meters; + float y_meters; + float heading_degs; +}; // Max range of range sensor. constexpr double RANGE_SENSOR_MAX_METERS = 4.0; @@ -34,8 +36,8 @@ struct sensor_data_t // Range on each radial. Range is -1 if there's no data. std::vector range_meters; - // 0 or more landmarks are visible - std::vector landmarks_visible; + // Radials for each measured range. + std::vector bearing_degs; }; -} // namespace utils +} // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 66250c1..8257985 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -61,7 +61,8 @@ void build_area_map(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); // and collision avoidance, and uses the area map for the map's path // boundary conditions. // Stores output 'working_map' record. -void build_working_map(gaia::slam::working_map_t&); +void build_working_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, + gaia::slam::working_map_t&); // Checks to see if it's time to select a new destination. If so, the @@ -87,7 +88,7 @@ void create_observation(const slam_sim::sensor_data_t& data); // pushed up from the sensor/hardware layer, but polling for it makes // the simulation more straightforward. void generate_sensor_data(double pos_x_meters, double pos_y_meters, - slam_sim::sensor_data_t& data); + sensor_data_t& data); // Generates and exports a map to disk. Filename will be based on ego's // timestamp. diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index bace2e1..14e7f43 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -1,20 +1,30 @@ +include ../Makefile.inc -all: - mkdir -p ../build && cd ../build/ && cmake .. && make -j 4 +# Make sure that we're not using g++. If so then environment variables +# aren't correctly set. +GXX_NAME = g++ +ifeq ($(CXX), $(GXX_NAME)) + $(error Using g++. Environment variables are not correctly configured) +endif -run: refresh_db - ../build/slam_sim -m ../data/map.json -x -3.0 -y 4.4 -drun: refresh_db - gdb --args ../build/slam_sim -m ../data/map.json -x -3.0 -y 4.4 +TARGET = slam_sim +OBJS = main.o analyze.o navigation.o occupancy.o rule_api.o +EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o +LIBS = /usr/local/lib/libgaia.so -pthread -lcurl -# At present there's no method to delete the contents of the database -# from w/in the program, so add a way to do so outside. -# NOTE: this may be implemented, but there may be a problem w/ that -# in regards to connected records -refresh_db: - gaiac ../gaia/slam.ddl --db-name slam -g -o /tmp/slam +all: $(OBJS) + $(CXX) $(OBJS) $(EXT_OBJS) -o $(TARGET) $(LIBS) + #cp $(TARGET) ../runtime/sim_shell + +%.o: %.cpp + $(CXX) $(INC) -c $(CPPFLAGS) $< + +refresh: clean all clean: - rm -f *.pgm + rm -f *.o $(TARGET) + rm -rf logs + + diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index 5b2854e..e662dfd 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -21,6 +21,7 @@ #include +#include "constants.hpp" #include "json.hpp" #include "line_segment.hpp" #include "sensor_data.hpp" @@ -93,10 +94,9 @@ void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { data.range_meters.clear(); data.bearing_degs.clear(); - data.num_radials = c_num_range_radials; - float step_degs = c_range_sensor_sweep_degs / (c_num_range_radials - 1); + float step_degs = c_range_sensor_sweep_degs / (data.num_radials - 1); // Get range on each radial, and store both distance and radial degs. - for (uint32_t n=0; ndata(); + m_grid = (map_node_t*) blob; + size_t flag_offset = num_nodes * sizeof *m_grid_flags; + m_grid_flags = (map_node_t*) blob[flag_offset]; + clear(); +} + + +occupancy_grid_t::occupancy_grid_t(working_map_t& wm) +{ + m_node_size_meters = c_area_map_node_width_meters; + // Get map size and bounds. + m_grid_size.rows = wm.num_rows(); + m_grid_size.cols = wm.num_cols(); + m_map_size.x_meters = wm.right_meters() - wm.left_meters(); + m_map_size.y_meters = wm.top_meters() - wm.bottom_meters(); + m_bottom_left.x_meters = wm.left_meters(); + m_bottom_left.y_meters = wm.bottom_meters(); + // Recover memory from blob cache. + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + m_blob_id = wm.blob_id(); + blob_t* blob = blob_cache_t::get_blob(m_blob_id); + if (blob == NULL) + { + // Blob doesn't exist yet. Create it. + size_t sz = num_nodes * (sizeof *m_grid + sizeof *m_grid_flags); + blob = blob_cache_t::create_or_reset_blob(wm.blob_id(), sz); + } + uint8_t* blob = (uint8_t*) blob->data(); + m_grid = (map_node_t*) blob; + size_t flag_offset = num_nodes * sizeof *m_grid_flags; + m_grid_flags = (map_node_t*) blob[flag_offset]; clear(); } + occupancy_grid_t::~occupancy_grid_t() { + if (m_blob_id < 0) + { + // Memory is owned by this. + free(m_grid); + free(m_grid_flags); + } } diff --git a/slam_sim/src/paths.cpp b/slam_sim/src/paths.cpp deleted file mode 100644 index 8afe979..0000000 --- a/slam_sim/src/paths.cpp +++ /dev/null @@ -1,559 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////////// -// -// Primary API for rules relating to path generation. -// -//////////////////////////////////////////////////////////////////////// - -#include -#include - -#include - -#include -#include - -#include "slam_sim.hpp" -#include "line_segment.hpp" -#include "landmark_description.hpp" - - -namespace slam_sim -{ - -extern int32_t g_quit; - -static int32_t next_observation_id = 1; - - -using gaia::common::gaia_id_t; - -using gaia::slam::observations_t; -using gaia::slam::ego_t; -using gaia::slam::paths_t; -using gaia::slam::destination_t; -using gaia::slam::estimated_position_t; -using gaia::slam::edges_t; -using gaia::slam::error_correction_t; -using gaia::slam::area_map_t; -using gaia::slam::local_map_t; -using gaia::slam::working_map_t; -using gaia::slam::landmark_sightings_t; -using gaia::slam::landmarks_t;; -using gaia::slam::sim_position_offset_t; -using gaia::slam::sim_actual_position_t; - -using gaia::slam::ego_writer; -using gaia::slam::paths_writer; -using gaia::slam::destination_writer; -using gaia::slam::estimated_position_writer; -using gaia::slam::edges_writer; -using gaia::slam::landmarks_writer; -using gaia::slam::sim_position_offset_writer; -using gaia::slam::sim_actual_position_writer; - -using utils::sensor_data_t; -using utils::landmark_description_t; - -//////////////////////////////////////////////////////////////////////// -// Rule API -// The functions here are expected to be called from within an active -// transaction - -// Dev code. Hardcode destinations during development. -constexpr double X_DEST_OFFSET_METERS = 8.0; -constexpr double Y_DEST_OFFSET_METERS = -8.8; - - -void select_destination() -{ - ego_t e = *(ego_t::list().begin()); - // Sanity check - paths_t path = e.current_path(); - assert(path.state() == PATH_STATE_ACTIVE); - // Estimate how long it should take to get to the destination. - estimated_position_t pos = e.position(); - double dx_meters = X_DEST_OFFSET_METERS; - double dy_meters = Y_DEST_OFFSET_METERS; - double dist_meters = sqrt(dx_meters*dx_meters + dy_meters*dy_meters); - if (dist_meters < INTER_OBSERVATION_DIST_METERS) - { - dist_meters = INTER_OBSERVATION_DIST_METERS; - } - double hops = ceil(dist_meters / INTER_OBSERVATION_DIST_METERS); - // Pad the arrival time in case we have to take a non-direct route. - hops *= 1.5; - // Set destination coordinates. - destination_t d = *(destination_t::list().begin()); - destination_writer writer = d.writer(); - writer.x_meters = pos.x_meters() + X_DEST_OFFSET_METERS; - writer.y_meters = pos.y_meters() + Y_DEST_OFFSET_METERS; - writer.expected_arrival = (int32_t) ceil(hops); - writer.update_row(); - gaia_log::app().info("Setting destination to {},{}", - d.x_meters(), d.y_meters()); -} - - -void select_landmark_destination() -{ - // Return to starting point - ego_t e = *(ego_t::list().begin()); - // Sanity check - paths_t path = e.current_path(); - assert((path.state() & PATH_STATE_STARTING) == 0); - assert((path.state() & PATH_STATE_DONE) == 0); - // Set 'find-landmark' state if it's not already set. - if (path.state() == PATH_STATE_ACTIVE) - { - paths_writer writer = path.writer(); - writer.state = PATH_STATE_FIND_LANDMARK; - writer.update_row(); - } - // Set destination coordinates. - sim_position_offset_t& spo = *(sim_position_offset_t::list().begin()); - destination_t d = *(destination_t::list().begin()); - destination_writer writer = d.writer(); - writer.x_meters = spo.dx_meters(); - writer.y_meters = spo.dy_meters(); - writer.update_row(); - gaia_log::app().info("Setting return destination to {},{}", - d.x_meters(), d.y_meters()); -} - - -// Initialize a path with the first observation. -void init_path(const observations_t& o) -{ - for (ego_t& e: ego_t::list()) - { - paths_t path = e.current_path(); - paths_writer writer = path.writer(); - writer.start_obs_id = o.id(); - writer.latest_obs_id = o.id(); - writer.num_observations = 1; - writer.state = slam_sim::PATH_STATE_ACTIVE; - writer.update_row(); - - // There's only one ego, but it doesn't hurt to break out anyway. - break; - } -} - - -void create_new_path() -{ - // Update ego with next path ID and get that ID to privide it to - // the new path. - uint32_t next_path_id = 0; - for (ego_t& e: ego_t::list()) - { - next_path_id = e.current_path_id() + 1; - ego_writer writer = e.writer(); - writer.current_path_id = next_path_id; - writer.update_row(); - break; - } - - // Create path. - paths_writer writer = paths_writer(); - writer.id = next_path_id; - writer.state = PATH_STATE_STARTING; - writer.insert_row(); - - // See if it's time to quit. - if (next_path_id >= EXIT_AFTER_X_PATHS) - { - g_quit = 1; - } -} - - -void full_stop() -{ - // Unimplemented. - // When a stop is requested we need to stop the wheels and then - // update our position. The simulated robot doesn't actually update - // its position in a smooth manner presently, instead jumping ahead - // to an interim destination. Once motion tracking is done smoothly - // over time then fill out this function. - assert(false); -} - - -// Update estimated position. This will trigger creation of a new -// observation. -// In a real robot, this would engage the wheels and drive a specified -// distance, in turn updating the estimated position. Here we just jump -// ahead to estimating the position. -void move_toward_destination() -{ - // TODO Consult map and find new checkpoint to move to. - // For now, move in the direction of the present destination. - // Update actual movement and perceived movement. - // For now these are one and the same . - // TODO Introduce error - - double dest_x_meters, dest_y_meters; - for (destination_t& d: destination_t::list()) - { - dest_x_meters = d.x_meters(); - dest_y_meters = d.y_meters(); - break; - } - double pos_x_meters, pos_y_meters; - estimated_position_writer writer; - for (estimated_position_t& ep: estimated_position_t::list()) - { - pos_x_meters = ep.x_meters(); - pos_y_meters = ep.y_meters(); - writer = ep.writer(); - break; - } - double dx = dest_x_meters - pos_x_meters; - double dy = dest_y_meters - pos_y_meters; - double dist = sqrt(dx*dx + dy*dy); - // Number of steps to get to destination. - double hops = dist / INTER_OBSERVATION_DIST_METERS; - if (hops < 1.0) - { - hops = 1.0; - } - dx /= hops; - dy /= hops; - writer.x_meters = pos_x_meters + dx; - writer.y_meters = pos_y_meters + dy; - writer.dx_meters = dx; - writer.dy_meters = dy; - writer.update_row(); -gaia_log::app().info("Moving from {},{} by {},{}", pos_x_meters, pos_y_meters, dx, dy); - - for (sim_actual_position_t& sap: sim_actual_position_t::list()) - { - sim_actual_position_writer sapw = sap.writer(); - sapw.x_meters = sap.x_meters() + dx; - sapw.y_meters = sap.y_meters() + dy; - sapw.update_row(); - } -} - - -//// There are 2 positional reference frames for Alice, that of the world -//// and that relative to Alice's starting point. The map is in world -//// coordinates while Alice doesn't know the world coordinates and so -//// uses the starting position as 0,0. This function returns the offset -//// from Alice's coordinate frame to the world coordinate frame. -//static void load_position_offset(double& dx_meters, double& dy_meters) -//{ -// // The positional offset doesn't change. Keep a cache of the offset -// // to avoid having to look it up each time. -// // Initial position must be positive as world map doesn't have -// // any negative coords. -// static double s_dx_meters = -1.0; -// static double s_dy_meters = -1.0; -// // -// if (s_dx_meters < 0.0) -// { -// for (sim_position_offset_t& spo: sim_position_offset_t::list()) -// { -// gaia_log::app().info("Reading position offset"); -// s_dx_meters = spo.dx_meters(); -// s_dy_meters = spo.dy_meters(); -// break; -// } -// } -// dx_meters = s_dx_meters; -// dy_meters = s_dy_meters; -//} - - -// Helper function when creating an observation, to store landmark -// sightings and create new landmark records if necessary. -static void update_landmarks(paths_t& path, double pos_x_meters, - double pos_y_meters, uint32_t obs_num, sensor_data_t& data) -{ - // Create landmark sighted records. - // Keep track of nearest one. - int32_t nearest_id = -1; - double nearest_meters = -1.0; - for (landmark_description_t& ld: data.landmarks_visible) - { - double dx = ld.x_meters - pos_x_meters; - double dy = ld.y_meters - pos_y_meters; - double range = sqrt(dx*dx + dy*dy); - double bearing = utils::R2D * atan2(dx, dy); - if (bearing < 0.0) - { - bearing += 360.0; - } - if ((nearest_meters < 0.0) || (range < nearest_meters)) - { - nearest_id = ld.id; - nearest_meters = range; - } - landmark_sightings_t::insert_row( - range, // range_meters - bearing, // bearing_degs - obs_num, // observaation_id - ld.id // landmark_id - ); - } - if ((path.num_observations() == 0) && (nearest_id < 0)) - { - std::cerr << "Start position must be near visible landmark.\n"; - exit(1); - } - // Create landmark records of sighted landmarks for those that don't - // exist. - for (landmark_description_t& ld: data.landmarks_visible) - { - assert(nearest_id >= 0); - landmarks_t mark = *(landmarks_t::list() - .where(gaia::slam::landmarks_expr::id == (uint32_t) ld.id).begin()); - if (!mark) - { -gaia_log::app().info("Creating landmark {} at {},{}", ld.name, ld.x_meters, ld.y_meters); - // Landmark record doesn't exist. Create it. - float confidence = 0.1; - if ((path.num_observations() == 0) && (ld.id == nearest_id)) - { - // This is the nearest landmark on the first observation. - // Use this as the anchor point. - confidence = 1.0; - assert(pos_x_meters == 0.0); - assert(pos_y_meters == 0.0); - } - landmarks_t::insert_row( - ld.id, // landmark_id - ld.name.c_str(), // description - ld.x_meters, // x_meters - ld.y_meters, // y_meters - confidence // confidence - ); - } - else if (mark.confidence() < 1.0) - { - // Update landmark position and confidence if sighting - // is closer to landmark than previous sighting(s). - double dx = ld.x_meters - pos_x_meters; - double dy = ld.y_meters - pos_y_meters; - double range = sqrt(dx*dx + dy*dy); - double landmark_dx = ld.x_meters - pos_x_meters; - double landmark_dy = ld.y_meters - pos_y_meters; - double landmark_range = - sqrt(landmark_dx*landmark_dx + landmark_dy*landmark_dy); - if (range < landmark_range) - { -gaia_log::app().info("Updating landmark {} to {},{}", ld.name, ld.x_meters, ld.y_meters); - // Update record. - landmarks_writer writer = mark.writer(); - writer.x_meters = dx; - writer.y_meters = dy; - writer.update_row(); - } - } - } -} - - -void create_observation(paths_t& path) -{ - // Get position and do a sensor sweep. - double actual_x_meters = -1.0; - double actual_y_meters = -1.0; - for (sim_actual_position_t& sap: sim_actual_position_t::list()) - { - actual_x_meters = sap.x_meters(); - actual_y_meters = sap.y_meters(); - break; - } - double pos_x_meters = -1.0; - double pos_y_meters = -1.0; - double dx_meters = -1.0; - double dy_meters = -1.0; - for (estimated_position_t& ep: estimated_position_t::list()) - { - pos_x_meters = ep.x_meters(); - pos_y_meters = ep.y_meters(); - dx_meters = ep.dx_meters(); - dy_meters = ep.dy_meters(); - break; - } - double heading_degs = utils::R2D * atan2(pos_x_meters, pos_y_meters); - double range_meters = sqrt(dx_meters*dx_meters + dy_meters*dy_meters); - gaia_log::app().info("Performing observation {} at {},{}", - next_observation_id, pos_x_meters, pos_y_meters); - sensor_data_t data; -// double x_offset_meters, y_offset_meters; -// load_position_offset(x_offset_meters, y_offset_meters); -// gaia_log::app().info("Position offset at {},{}", -// x_offset_meters, y_offset_meters); -printf("SENSOR sweep for obs %d\n", next_observation_id); - perform_sensor_sweep(actual_x_meters, actual_y_meters, data); - - // Create an observation record, storing sensor data. - // This is the ID of the new observation. Call it 'num' here so to not - // get confused with gaia IDs. - uint32_t obs_num = next_observation_id++; - if (path.num_observations() == 0) - { - // For first observation, don't store position delta. - dx_meters = 0.0; - dy_meters = 0.0; - heading_degs = 0.0; - range_meters = 0.0; - } - gaia_id_t new_obs_id = observations_t::insert_row( - obs_num, // id - pos_x_meters, // pos_x_meters - pos_y_meters, // pos_y_meters - actual_x_meters, // actual_x_meters - actual_y_meters, // actual_y_meters - dx_meters, // dx_meters - dy_meters, // dy_meters - heading_degs, // heading_degs - range_meters, // dist_meters - data.num_radials, // num_radials - data.range_meters // distance_meters - ); - observations_t new_obs = observations_t::get(new_obs_id); - - update_landmarks(path, pos_x_meters, pos_y_meters, obs_num, data); - - // Connect observation to path and to previous observation, if present. - // Make a copy of the number of observations while we modify path. - uint32_t number_of_observations = path.num_observations(); - paths_writer p_writer = path.writer(); - if (number_of_observations == 0) - { - // First observation. - p_writer.start_obs_id = obs_num; - // For first observation, don't store position delta. - dx_meters = 0.0; - dy_meters = 0.0; - heading_degs = 0.0; - range_meters = 0.0; - } - else - { - // Build an edge to connect to the previous observation. - gaia_id_t edge_id = edges_t::insert_row(obs_num); - // Now link observations to the edge. - new_obs.reverse_edge().connect(edge_id); - if (number_of_observations == 1) - { - // Second observation so this is the first edge. Get - // connection info directly from - observations_t prev_obs = path.first_observation(); - prev_obs.forward_edge().connect(edge_id); - } - else - { - // Get connection info from previous edge. - observations_t prev_obs = path.latest_observation(); - prev_obs.forward_edge().connect(edge_id); - } - - } - p_writer.latest_obs_id = obs_num; - p_writer.num_observations = path.num_observations() + 1; - p_writer.update_row(); -} - - -//////////////////////////////////////////////////////////////////////// -// Non-rule API -// The functions here must manage their own transactions. - -void request_new_destination(double x_meters, double y_meters) -{ - (void) x_meters; - (void) y_meters; - // Not implemented yet. - assert(false); -} - - -void seed_database(double initial_x_meters, double initial_y_meters) -{ - // There shouldn't be any transaction conflicts as this is the first - // operation on the database, so ignore the try/catch block. - gaia::db::begin_transaction(); - // - gaia_id_t ego_id = ego_t::insert_row(0); // current_path_id - ego_t ego = ego_t::get(ego_id); - - //////////////////////////////////////////// - // Position and destination - gaia_id_t destination_id = destination_t::insert_row( - 0.0, // x_meters - 0.0, // y_meters - 0 // expected_arrival - ); - gaia_id_t position_id = estimated_position_t::insert_row( - 0.0, // x_meters - 0.0, // y_meters - 0.0, // x_meters - 0.0 // y_meters - ); - gaia_id_t error_correction_id = error_correction_t::insert_row( - 0.0, // drift_correction - 0.0, // forward_correction - 0.0 // correction_weight - ); - - //////////////////////////////////////////// - // Maps - gaia_id_t area_map_id = area_map_t::insert_row( - 0.0, // left_meters - 0.0, // right_meters - 0.0, // top_meters - 0.0, // bottom_meters - 0 // change_counter - ); - gaia_id_t local_map_id = local_map_t::insert_row( - 0.0, // left_meters - 0.0, // right_meters - 0.0, // top_meters - 0.0, // bottom_meters - 0 // change_counter - ); - gaia_id_t working_map_id = working_map_t::insert_row( - 0 // change_counter - ); - - //////////////////////////////////////////// - // Simulation interface - sim_position_offset_t::insert_row( - initial_x_meters, // dx_meters - initial_y_meters // dy_meters - ); - - sim_actual_position_t::insert_row( - initial_x_meters, // x_meters - initial_y_meters // y_meters - ); - - - //////////////////////////////////////////// - // Establish relationships - ego.position().connect(position_id); - ego.destination().connect(destination_id); - ego.error().connect(error_correction_id); - ego.low_res_map().connect(area_map_id); - ego.high_res_map().connect(local_map_id); - ego.working_map().connect(working_map_id); - - //////////////////////////////////////////// - // All done - gaia::db::commit_transaction(); -} - -} // namespace slam_sim diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 26485e8..cf4be82 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -108,17 +108,29 @@ static void write_map_to_file(occupancy_grid_t& map) } -void build_map() +void export_map_to_file() { gaia::db::begin_transaction(); ego_t ego = *(ego_t::list().begin()); observed_area_t& area = *(observed_area_t::list().begin()); - build_map(ego.current_graph(), area); + // TODO remove padding from bounds before printing + + + world_coordinate_t top_left = { + .x_meters = area.left_meters(), + .y_meters = area.top_meters() + }; + world_coordinate_t bottom_right = { + .x_meters = area.right_meters(), + .y_meters = area.bottom_meters() + }; + build_map(ego.current_graph(), top_left, bottom_right); gaia::db::commit_transaction(); } -void build_map(const graphs_t& g, const observed_area_t& bounds) +void build_map(const graphs_t& g, const world_coordinate_t& top_left, + const world_coordinate_type bottom_right) { world_coordinate_t top_left = { .x_meters = bounds.left_meters(), diff --git a/slam_sim/src/utils/Makefile b/slam_sim/src/utils/Makefile index 5c0a329..61aaf3f 100644 --- a/slam_sim/src/utils/Makefile +++ b/slam_sim/src/utils/Makefile @@ -2,7 +2,7 @@ include ../../Makefile.inc TEST_TARGETS = test_line_segment -OBJS = line_segment.o landmark_description.o blob_cache.o +OBJS = line_segment.o blob_cache.o all: $(OBJS) diff --git a/slam_sim/src/utils/landmark_description.cpp b/slam_sim/src/utils/landmark_description.cpp deleted file mode 100644 index 68754f4..0000000 --- a/slam_sim/src/utils/landmark_description.cpp +++ /dev/null @@ -1,38 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////////// -// -// Landmarks are uniqely identifiable locations in the environment. -// The description stores their position and an identifier. -// -//////////////////////////////////////////////////////////////////////// - -#include "landmark_description.hpp" - -namespace utils -{ - -landmark_description_t::landmark_description_t() -{ - this->name = "undefined"; - this->id = -1; - this->x_meters = 0.0; - this->y_meters = 0.0; -} - -landmark_description_t::landmark_description_t(std::string name, int32_t id, - double x_meters, double y_meters) -{ - this->name = name; - this->id = id; - this->x_meters = x_meters; - this->y_meters = y_meters; -} - -} // namespace utils diff --git a/slam_sim/src/utils/line_segment.cpp b/slam_sim/src/utils/line_segment.cpp index 696395a..149fd6d 100644 --- a/slam_sim/src/utils/line_segment.cpp +++ b/slam_sim/src/utils/line_segment.cpp @@ -24,7 +24,7 @@ using namespace std; -namespace utils +namespace slam_sim { @@ -133,5 +133,5 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) } -} // namespace utils +} // namespace slam_sim From fd3fc8f36438fafea8f6c4308e1b08b11a7a7cfa Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Mon, 4 Apr 2022 13:21:00 -0700 Subject: [PATCH 08/30] Compiling again --- slam_sim/gaia/Makefile | 27 +++++++++------ slam_sim/include/constants.hpp | 14 +++++--- slam_sim/src/Makefile | 10 ++++-- slam_sim/src/globals.cpp | 16 --------- slam_sim/src/main.cpp | 10 ++++-- slam_sim/src/occupancy.cpp | 34 ++++++++++--------- slam_sim/src/rule_api.cpp | 61 +++++++++++++++++----------------- 7 files changed, 89 insertions(+), 83 deletions(-) delete mode 100644 slam_sim/src/globals.cpp diff --git a/slam_sim/gaia/Makefile b/slam_sim/gaia/Makefile index d892f60..2d40a8d 100644 --- a/slam_sim/gaia/Makefile +++ b/slam_sim/gaia/Makefile @@ -1,25 +1,30 @@ -# Copyright (c) Gaia Platform LLC -# -# Use of this source code is governed by the MIT -# license that can be found in the LICENSE.txt file -# or at https://opensource.org/licenses/MIT. - -# The purpose of this makefile is to simply execute gaiac and gaiat -# to make sure there are not errors (without executing higher level -# make). The single target then cleans up after itself. +include ../Makefile.inc RULES_CPP = slam_rules.cpp DB_NAME = slam -INC = -I/opt/gaia/include/ -Islam/ -I../include/ OBJS = slam_rules.o slam/gaia_slam.o -all: +all: all_gaia all_procedure_subshell + +all_gaia: gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME) gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) + + +# when freshly making, slam_rules.cpp may not exist (e.g., if 'make clean' +# was just run). drop into subshell for making local procedural files +# as that will induce a scan of local files +all_procedure_subshell: + make all_procedure + +all_procedure: $(OBJS) refresh: clean all +%.o: %.cpp + $(CXX) $(INC) -c $(CPPFLAGS) $< + clean: rm -f *.o $(RULES_CPP) rm -rf $(DB_NAME) diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 5bd2b5e..347a2da 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -4,17 +4,21 @@ namespace slam_sim { // Degree/Radian conversions. -constexpr float c_rad_to_deg = 180.0 / M_PI; -constexpr float c_deg_to_rad = M_PI / 180.0; +constexpr float c_rad_to_deg = (float) (180.0 / M_PI); +constexpr float c_deg_to_rad = (float) (M_PI / 180.0); // Width (height) of grids in occupancy grid. -constexpr float c_map_node_width_meters = 0.1; +constexpr float c_map_node_width_meters = 0.1f; // Max distance range sensors can get range data. -constexpr float c_range_sensor_max_meters = 4.0; +constexpr float c_range_sensor_max_meters = 4.0f; // Width of range sensor sweep in front of bot. -constexpr float c_range_sensor_sweep_degs = 90.0; +constexpr float c_range_sensor_sweep_degs = 90.0f; constexpr int32_t c_num_range_radials = 45; +constexpr float c_area_map_node_width_meters = 0.25f; +constexpr float c_working_map_node_width_meters = 0.05f; +constexpr float c_standard_map_node_width_meters = 0.05f; + } // namespace slam_sim diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 14e7f43..f8989af 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -10,20 +10,26 @@ endif TARGET = slam_sim -OBJS = main.o analyze.o navigation.o occupancy.o rule_api.o +OBJS = main.o analyze.o occupancy.o rule_api.o EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o -LIBS = /usr/local/lib/libgaia.so -pthread -lcurl +LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl all: $(OBJS) + cd utils && make $(CXX) $(OBJS) $(EXT_OBJS) -o $(TARGET) $(LIBS) #cp $(TARGET) ../runtime/sim_shell +run: + ./$(TARGET) -m ../data/map.json -x -3.0 -y -3.0 + + %.o: %.cpp $(CXX) $(INC) -c $(CPPFLAGS) $< refresh: clean all clean: + cd utils && make clean rm -f *.o $(TARGET) rm -rf logs diff --git a/slam_sim/src/globals.cpp b/slam_sim/src/globals.cpp deleted file mode 100644 index 66ab47f..0000000 --- a/slam_sim/src/globals.cpp +++ /dev/null @@ -1,16 +0,0 @@ -//////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////// - -#include - -namespace slam_sim -{ - -int32_t g_quit = 0; - -} diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 5253c46..dd221e6 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -28,7 +28,13 @@ namespace slam_sim using std::this_thread::sleep_for; -constexpr uint32_t c_rule_wait_millis = 100; +static constexpr uint32_t c_rule_wait_millis = 100; + + +// Globals for this namespace +int32_t g_quit = 0; +float g_initial_x_meters = 0.0; +float g_initial_y_meters = 0.0; /** @@ -58,7 +64,7 @@ void remove_all_rows() } -void clear_data() +void clean_db() { gaia::db::begin_transaction(); remove_all_rows(); diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 8ed4bdb..88605a8 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -18,6 +18,8 @@ #include #include +#include "blob_cache.hpp" +#include "constants.hpp" #include "line_segment.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" @@ -60,8 +62,8 @@ occupancy_grid_t::occupancy_grid_t(float node_width_meters, uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; // Signal that this owns the memory allocation. m_blob_id = -1; - m_grid = malloc(num_nodes * sizeof *m_grid); - m_grid_flags = malloc(num_nodes * sizeof *m_grid_flags); + m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); + m_grid_flags = (map_node_flags_t*) malloc(num_nodes * sizeof *m_grid_flags); clear(); } @@ -79,24 +81,23 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am) // Recover memory from blob cache. uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; m_blob_id = am.blob_id(); - blob_t* blob = blob_cache_t::get_blob(m_blob_id); + blob_t* blob = blob_cache_t::get()->get_blob(m_blob_id); if (blob == NULL) { // Blob doesn't exist yet. Create it. size_t sz = num_nodes * (sizeof *m_grid + sizeof *m_grid_flags); - blob = blob_cache_t::create_or_reset_blob(am.blob_id(), sz); + blob = blob_cache_t::get()->create_or_reset_blob(am.blob_id(), sz); } - uint8_t* blob = (uint8_t*) blob->data(); - m_grid = (map_node_t*) blob; + m_grid = (map_node_t*) blob->data; size_t flag_offset = num_nodes * sizeof *m_grid_flags; - m_grid_flags = (map_node_t*) blob[flag_offset]; + m_grid_flags = (map_node_flags_t*) &blob->data[flag_offset]; clear(); } occupancy_grid_t::occupancy_grid_t(working_map_t& wm) { - m_node_size_meters = c_area_map_node_width_meters; + m_node_size_meters = c_working_map_node_width_meters; // Get map size and bounds. m_grid_size.rows = wm.num_rows(); m_grid_size.cols = wm.num_cols(); @@ -107,17 +108,16 @@ occupancy_grid_t::occupancy_grid_t(working_map_t& wm) // Recover memory from blob cache. uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; m_blob_id = wm.blob_id(); - blob_t* blob = blob_cache_t::get_blob(m_blob_id); + blob_t* blob = blob_cache_t::get()->get_blob(m_blob_id); if (blob == NULL) { // Blob doesn't exist yet. Create it. size_t sz = num_nodes * (sizeof *m_grid + sizeof *m_grid_flags); - blob = blob_cache_t::create_or_reset_blob(wm.blob_id(), sz); + blob = blob_cache_t::get()->create_or_reset_blob(wm.blob_id(), sz); } - uint8_t* blob = (uint8_t*) blob->data(); - m_grid = (map_node_t*) blob; + m_grid = (map_node_t*) blob->data; size_t flag_offset = num_nodes * sizeof *m_grid_flags; - m_grid_flags = (map_node_t*) blob[flag_offset]; + m_grid_flags = (map_node_flags_t*) &blob->data[flag_offset]; clear(); } @@ -160,11 +160,12 @@ void map_node_flags_t::clear() void occupancy_grid_t::clear() { - for (uint32_t i=0; i Date: Mon, 4 Apr 2022 21:53:05 -0700 Subject: [PATCH 09/30] Porting route planning code for SLAM --- slam_sim/include/constants.hpp | 6 + slam_sim/include/occupancy.hpp | 30 +- slam_sim/src/occupancy.cpp | 5 + slam_sim/src/utils/path_map.cpp | 478 ++++++++++++++++++++++++++++++++ 4 files changed, 515 insertions(+), 4 deletions(-) create mode 100644 slam_sim/src/utils/path_map.cpp diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 347a2da..29a5f9f 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -20,5 +20,11 @@ constexpr float c_area_map_node_width_meters = 0.25f; constexpr float c_working_map_node_width_meters = 0.05f; constexpr float c_standard_map_node_width_meters = 0.05f; +// Path finding. +// Bias factor for seeking out new areas to explore. This is the increase +// in cost to traverse a node for each time it's been observed. +constexpr float c_path_penalty_per_observation = 0.05f; + + } // namespace slam_sim diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 34593d8..6fb9e4c 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -20,6 +20,7 @@ As elsewhere in the code, positive coordinates are to the up and right. ***********************************************************************/ #include +#include #include #include "gaia_slam.h" @@ -50,9 +51,10 @@ struct map_node_t // node's characteristics. float traversal_cost; - // Total cost to reach destination. - float distance; - + // Total cost to reach destination. This is a unitless measure that + // includes biases to avoid heavily trafficed areas to encourage + // exploration + float path_cost; void clear(); }; @@ -97,6 +99,19 @@ struct map_size_t }; +struct node_offset_t +{ + int32_t dx; + int32_t dy; +}; + +struct grid_index_t +{ + uint32_t idx; +}; + + + class occupancy_grid_t { public: @@ -106,7 +121,7 @@ class occupancy_grid_t occupancy_grid_t(gaia::slam::working_map_t&); ~occupancy_grid_t(); - // Initialization + / Initialization // Resets the map grid. void clear(); // Initializes as inner map using outer map to set boundary conditions. @@ -148,6 +163,13 @@ class occupancy_grid_t //std::vector m_grid; //std::vector m_grid_flags; + //////////////////////////////////////////// + // Path-finding. This is an algorithm derived from D*, ported from + // different project (used w/ permission). + std::queue m_queue; + void add_node_to_stack(map_node* root_node, grid_index_t root_idx, + node_offset_t offset, const float traverse_wt = 1.0f); + world_coordinate_t m_destination; }; diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 88605a8..2b42b79 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -39,6 +39,11 @@ using gaia::slam::area_map_t;; using gaia::slam::working_map_t;; +// During porting, pull in the file inline. Once it's cleaned up and +// compiles, merge the contents in here. +#include "utils/path_map.cpp" + + //////////////////////////////////////////////////////////////////////// // Constructors and destructors diff --git a/slam_sim/src/utils/path_map.cpp b/slam_sim/src/utils/path_map.cpp new file mode 100644 index 0000000..dc5b8c2 --- /dev/null +++ b/slam_sim/src/utils/path_map.cpp @@ -0,0 +1,478 @@ +/*********************************************************************** +* Copyright (C) 2019-2022 Keith Godfrey +* Copyright (C) 2022 Gaia +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +***********************************************************************/ +#include +#include +#include +#include + +#include "occupancy.hpp" + + +static bool is_not_passable(map_node_t& child_node) +{ + if (child_node.boundary > 0.0) + { + // A boundary was observed in this node. If it's not a fluke, + // it's not passable. + assert(child_node.observed > 0.0); + if (child_node.boundary / child_node.observed > 0.1) + { + return; + } + } +} + + +// if node at root+offset belongs as part of path, sets node values +// (eg, weight and link to parent) +// add pixel to stack for future neighbor analysis +// 'distance' is the cost to reach this node from a neighbor (i.e., 1.0 or +// 1.4, depending if it's NSEW or diagonal neighbor) +void occupancy_grid_t::add_node_to_stack( + /* in */ const map_node_t& root_node, + /* in */ const grid_index_t root_idx, + /* in */ const node_offset_t offset, + /* in */ const float traversal_wt + ) +{ + // make sure we're not going off the edge of the map + int32_t new_x = (int32_t) root_node.pos.x + offset.dx; + int32_t new_y = (int32_t) root_node.pos.y + offset.dy; + if ((new_x < 0) || (new_x >= m_grid_size.cols) || (new_y < 0) || + (new_y >= m_grid_size.rows)) + { + return; + } + ///////////////////////////////////////////////////////////////////// + // point is in the world -- check it + uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); + map_node_t& child_node = grid->nodes[new_idx]; + if (is_not_passable(child_node)) + { + return; + } + // Determine weight for traversing this node. Have lower weight to nodes + // infrequently observed/traversed to provide bias to take different + // routes. + float weight = 1.0f; + weight += child_node.observed * c_path_penalty_per_observation; + // Path tracing algorithm is deterministic and can follow vert or + // horiz path too easily. Add some jitter to allow pulling in + // from more accurate direction. + double jitter = 0.0; + drand48_r(&path_rand_, &jitter); + jitter = 0.1 * (jitter - 0.5); + + float new_weight = root_node.weight + penalty + traversal_wt + + (float) jitter; + if (child_node->flags & PATH_NODE_FLAG_PROCESSED) { + // already-processed node + // see if this path might provide it a lower weight + if (child_node->weight <= new_weight) + { + // Nope. + return; + } + // new weight is lower -- allow to be added to stack again to propagate + // updated weight to neighbors + } + // add point to stack for + child_node->parent_id = root_idx; + child_node->weight = new_weight; + child_node->passage_penalty = penalty; + child_node->flags |= PATH_NODE_FLAG_PROCESSED; + // + m_queue.push(new_idx); +end: + ; +} + + +// if node at root+offset belongs as part of path, sets node values +// (eg, weight and link to parent) +// for node that's diagonal, include weight of 4-connected neighbor required +// to reach diagonal node +// add pixel to stack for future neighbor analysis +void occupancy_grid_t::add_node_to_stack_diag( + /* in */ const map_node_t& root_node, + /* in */ const grid_index_t root_idx, + /* in */ const node_offset_t offset + ) +{ + // make sure we're not going off the edge of the map + int32_t new_x = (int32_t) root_node.pos.x + offset.dx; + int32_t new_y = (int32_t) root_node.pos.y + offset.dy; + if ((new_x < 0) || (new_x >= m_grid_size.cols) + || (new_y < 0) || (new_y >= m_grid_size.rows)) + { + goto end; + } + ///////////////////////////////////////////////////////////////////// + // point is in the world -- check it + uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); + map_node_t& child_node = m_grid[new_idx]; + if (is_not_passable(child_node)) + { + return; + } + + // see if there's a way to get to this diagonal node + // check via vertical and horizontal neighbor + uint32_t idx_vert = + (uint32_t) (root_node.pos.x + new_y * m_grid_size.cols); + uint32_t idx_horiz = + (uint32_t) (new_x + root_node.pos.y * m_grid_size.cols); + map_node_t *vert_child_node = &m_grid[idx_vert]; + map_node_t *horiz_child_node = &m_grid[idx_horiz]; + + if ((vert_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) && + (horiz_child_node->flags & PATH_NODE_FLAG_NO_ACCESS)) { + // no direct 4-connected path. nothing to do here + goto end; + } + ///////////////////////////////////////////////////////////////////// + // point is reachable. check weight. this will be combination of + // weight from easiest 4-connected path to get to diagonal node + // and of diagonal node itself + float nbr_penalty = -1.0f; + // get lowest traversal weight of vert and horiz paths and use that as + // base for weight to diagonal node + if ((vert_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) == 0) { + nbr_penalty = vert_child_node->passage_penalty; + } + if ((horiz_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) == 0) { + float penalty = horiz_child_node->passage_penalty; + if (nbr_penalty < 0.0f) { + // not accessible by vert node, but horiz provides path. use + // horiz penalty + nbr_penalty = penalty; + } else if ((penalty > 0.0f) && (penalty < nbr_penalty)) { + // horiz node is passable and penalty is less than via vert, so + // use horiz's penalty + nbr_penalty = penalty; + } + } + if (nbr_penalty < 0.0f) { + // no direct 4-connected path. nothing to do here + goto end; + } + // weight needed to traverse neighboring node + // diagonal weight should be 1.414, but there appears to be a strong + // bias for horiz/vert travel. lower weight to give diagonal more + // preference, to balance things out + float traversal_weight = nbr_penalty + 1.25f; + add_node_to_stack(root_node, root_idx, offset, traversal_weight); +end: + ; +} + +// pixel neighbors +// 4-connected base +static const node_offset_t OFF_E = { .dx= 1, .dy= 0 }; +static const node_offset_t OFF_W = { .dx=-1, .dy= 0 }; +static const node_offset_t OFF_N = { .dx= 0, .dy=-1 }; +static const node_offset_t OFF_S = { .dx= 0, .dy= 1 }; +// 8-connected extras +static const node_offset_t OFF_NE = { .dx= 1, .dy=-1 }; +static const node_offset_t OFF_NW = { .dx=-1, .dy=-1 }; +static const node_offset_t OFF_SE = { .dx= 1, .dy= 1 }; +static const node_offset_t OFF_SW = { .dx=-1, .dy= 1 }; + + +// pop the next node off the stack and processess it +static void occupancy_grod_t::process_next_stack_node() +{ + // add adjacent nodes to stack + grid_index_t root_idx = m_queue.pop(); + map_node_t& root_node = m_grid[root_idx.idx]; + // + add_node_to_stack(root_node, root_idx, OFF_E); + add_node_to_stack(root_node, root_idx, OFF_W); + add_node_to_stack(root_node, root_idx, OFF_N); + add_node_to_stack(root_node, root_idx, OFF_S); + // add 8-connected nodes if there's a valid 2-step 4-conneced path + // to get there + add_node_to_stack_diag(root_node, root_idx, OFF_NE); + add_node_to_stack_diag(root_node, root_idx, OFF_NW); + add_node_to_stack_diag(root_node, root_idx, OFF_SE); + add_node_to_stack_diag(root_node, root_idx, OFF_SW); +} + + +// build vector of approximate course to follow for all nodes in path map. +// present algorithm is very simple and is based on direction to 'grandparent' +// node, approx. 5 'generations' away +// TODO present implementation gives very poor angular accuracy and misses +// turns, potentially aiming it toward land or across shallow water. +// route calculation should compensate for this but it should be fixed +// here at the source. build more accurate direction vectors for each +// path map node +// TODO migrate away from vectors and only calculate course +// also calculates course from vector +// TODO re-evaluate and fix for use in arctic ocean +static void build_course_vectors( + /* in out */ occupancy_grid_t *path_map, + /* in */ const degree_type center_latitude + ) +{ + image_size_type size = path_map->size; + // build direction vector for each node + double dy_deg = 1.0 / (double) size.y; + double y_deg_top = center_latitude.degrees - dy_deg * (double) (size.y / 2); + pixel_offset_bitfield_type base_direction, next_direction; + // + for (uint32_t y=0; ynodes[idx]; + map_node_t *ggp = root; +//printf("%d,%d scale=%.4f\n", root->pos.x, root->pos.y, scale); + // find direction to ancestor + for (uint32_t gen=0; genparent_id.val >= 0) { + map_node_t *next_ggp = + &path_map->nodes[ggp->parent_id.idx]; + if (gen == 0) { + // get base direction. this returns bitfield indicating + // direction of next node relative to this one + base_direction = get_offset_mask(next_ggp->pos, ggp->pos); + } else { + // make sure offset is consistent w/ base direction + // (ie, w/in 45deg) + // get new direction. this returns bitfield indicating + // direction of next node relative to this one, + // plus adjacent bits. ANDing this to base direction + // will indicate if it's w/in +/-45deg of base + next_direction = + get_offset_mask_wide(next_ggp->pos, ggp->pos); + if ((next_direction.mask & base_direction.mask) == 0) { + // latest direction is too different from original + // offset. halt search + break; + } + } + ggp = next_ggp; +//printf(" ->%d,%d %.3f\n", ggp->pos.x, ggp->pos.y, ggp->passage_penalty); + } else { + break; + } + } + path_offset_type dir; + dir.dx = (int16_t) (ggp->pos.x - root->pos.x); + dir.dy = (int16_t) (ggp->pos.y - root->pos.y); + double theta_deg = R2D * atan2((double) dir.dx * scale, (double) -dir.dy); +//printf(" = %d,%d for %.3f (x-scale= %.3f, wt=%.3f)\n", dir.dx, dir.dy, theta_deg, scale, ggp->weight); + CVT_DEG_TO_BAM16(theta_deg, root->true_course); + // don't need to reset active course -- it's set when smoothing + // out course + // active course is reset when smoothing course, but not for all + // all nodes (ie, boundary nodes). take care of those cases here + root->active_course = root->true_course; +//printf("INIT %d %d,%d [%d] dir %d,%d %.2f (active %.2f)\n", idx, x, y, root->parent_id.val, dir.dx, dir.dy, (double) root->true_course.angle16 * BAM16_TO_DEG, root->active_course.angle16 * BAM16_TO_DEG); + } + } +} + + +// adds point (ie, destination or beacon) to path map, using specified +// path weight +void occupancy_grid_t::add_destination_to_path_stack( + /* in */ const image_coordinate_type pos, + /* in */ const float path_weight + ) +{ + // make sure pixel is in map + if ((pos.x < MAP_LEVEL3_SIZE) || (pos.y < MAP_LEVEL3_SIZE)) { + uint32_t idx = (uint32_t) (pos.x + pos.y * MAP_LEVEL3_SIZE); + map_node_t* path_node = m_grid[idx]; + path_node.weight = path_weight; + path_node.parent_id.val = -1; + m_queue.push(idx); + path_node->flags = PATH_NODE_FLAG_PROCESSED; +//fprintf(stderr, "Seeded %d,%d with %.1f\n", pos.x, pos.y, (double) path_weight); + } +} + +// use D* approach to find all routes to destination +// path traced on existing depth map, using beacons and destination as +// seed locations +void trace_route_simple( + /* out */ occupancy_grid_t *path_map, + /* in */ const world_coordinate_type vessel_pos + ) +{ +//printf("TRACE SIMPLE %d beacons\n", path_map->num_beacons); + reset_path_map(path_map); +// path_map->destination = convert_latlon_to_akn(path_map->center); + // add beacon and dest weights to map, as able + path_map->read_idx = 0; + path_map->write_idx = 0; + // add destination as primary seed with 0 weight + // if destination is beyond visible map then it will be filtered and + // not actually added stack + calculate_destination_map_position(path_map); + add_point_to_path_stack(path_map, path_map->dest_pix, 0.0f); + // add beacons with seeds of their computed path weights to destination + for (uint32_t i=0; inum_beacons; i++) { + // TODO don't add beacon if w/in X distance of vessel. that's too + // close and likely to result in artifacts + map_beacon_reference_type *ref = &path_map->beacon_ref[i]; + world_coordinate_type beac_pos = convert_akn_to_world(ref->coords); + meter_type dist = calc_distance(vessel_pos, beac_pos, __func__); + if (dist.meters < VESSEL_BEACON_INHIBITION_RING_NM * NM_TO_METERS) { + // beacon is too close to vessel. inhibit its placement in map + continue; + } + float weight = path_map->beacon_ref[i].path_weight; + if (weight > 0.0f) { +//printf(" adding beacon %d to stack (wt=%.1f)\n", path_map->beacon_ref[i].index, (double) weight); + // if using beacon weights directly, variations in paths can + // result in a beacon being a local minimum that the path + // cannot escape from. to fix this problem, multiply all + // beacon weights by a constant (e.g., 2x). this way the + // the lowest weight beacon in a map will be the one that + // has the strongest attraction, as it should overcome the + // weight of all beacons between it and the vessel. this way + // we can avoid the local minimum problem without complicated + // logic that determines the best beacon to drive toward, and + // which are of lower priority (ie, are closer) while covering + // for all terrain variations + add_point_to_path_stack(path_map, + path_map->beacon_ref[i].pos_in_map, 2.0f * weight); + } +//else { printf(" beacon %d has wt=%.1f\n", path_map->beacon_ref[i].index, weight); } + } + // update path weight between nodes until stack is empty + uint32_t num_nodes = MAP_LEVEL3_SIZE * MAP_LEVEL3_SIZE; +//uint32_t cnt = 0; + while (path_map->read_idx < path_map->write_idx) { +//cnt++; + // because nodes can be added to the stack multiple times it's + // possible for stack to grow beyond initial size (which is + // number of nodes). if stack overflows, purge used contents + // and shrink it + if (path_map->write_idx >= num_nodes) { + shrink_path_stack(path_map); + } + process_next_stack_node(path_map); + } +//printf("Processed %d nodes\n", cnt); + degree_type center_latitude = { .degrees = path_map->center.latitude }; + build_course_vectors(path_map, center_latitude); +} + + +// generate map based on existing map, with map center being X-nm in +// front of vessel with vessel following existing path map's route +// to get to destination +void rebuild_map_by_vessel_offset( + /* in out */ occupancy_grid_t *path_map, + /* in */ const image_coordinate_type vessel_pix, + /* in */ const world_coordinate_type vessel_pos + ) +{ +printf("Rebuild map by vessel offset\n"); + uint32_t idx = (uint32_t) (vessel_pix.x + vessel_pix.y * path_map->size.x); + map_node_t *node = &path_map->nodes[idx]; + // if node doesn't have direction information, that's an error, but + // tolerate it. can't offset if this node has no path info, so + // center map on vessel + if (node->weight < 0.0f) { + // can't generate offset if vessel's node has no path info, so just + // center map on vessel +fprintf(stderr, "Generating offset map, but vessel pix %d,%d in old map has no path info. Vessel at %.4f,%.4f\n", vessel_pix.x, vessel_pix.y, vessel_pos.lon, vessel_pos.lat); + log_err(log_, "Generating offset map, but vessel pix %d,%d in old " + "map has no path info. Vessel at %.4f,%.4f", + vessel_pix.x, vessel_pix.y, vessel_pos.lon, vessel_pos.lat); + // +printf(" (no path info)\n"); + load_world_5sec_map(vessel_pos, path_map); + } else { + // get direction vessel should be moving and build map based on that + // direction to head to reach destination + degree_type course = { .degrees = + (double) node->true_course.angle16 * BAM16_TO_DEG }; + // calc where map center should be, in that direction + meter_type dist = + { .meters = VESSEL_OFFSET_FROM_MAP_CENTER_NM * NM_TO_METERS }; + world_coordinate_type new_center = + calc_offset_position(vessel_pos, course, dist); + // load map + load_world_5sec_map(new_center, path_map); + } + load_beacons_into_path_map(path_map); +//write_depth_map(path_map, "c.pnm"); + trace_route_simple(path_map, vessel_pos); + path_map->vessel_start_pix = get_pix_position_in_map(path_map, + vessel_pos); +//write_depth_map(path_map, "d.pnm"); +} + + +// initial beacon trace for route plus determining appropriate world +// map center +// performs several traces. first beacons are traced. then trace is +// made with vessel at center, then map is moved and trace is made +// with vessel offset X-nm from center (e.g., 15nm) and direction +// of travel moving through center +// returns 0 on success and -1 on failure +int trace_route_initial( + /* in out */ occupancy_grid_t *path_map, + /* in */ const world_coordinate_type dest, + /* in */ const world_coordinate_type vessel_pos + ) +{ + // TODO either use constants or use size. right now these are + // unreasonably intermixed + assert(path_map->size.x == MAP_LEVEL3_SIZE); + assert(path_map->size.y == MAP_LEVEL3_SIZE); + //////////////// + int rc = -1; + log_info(log_, "Computing routes from %.5f,%.5f to %.5f,%.5f", + vessel_pos.x_deg, vessel_pos.y_deg, dest.x_deg, dest.y_deg); + world_coordinate_type destination = dest; + if (destination.lon < 0.0) { + destination.lon += 360.0; + } + check_world_coordinate(destination, __func__); + world_coordinate_type ves_pos = vessel_pos; + if (ves_pos.lon < 0.0) { + ves_pos.lon += 360.0; + } + check_world_coordinate(ves_pos, __func__); + path_map->destination = convert_latlon_to_akn(destination); + // construct beacon path map + if (trace_beacon_paths(path_map, dest) != 0) { + // something bad happened. error should have been reported from + // w/in function itself + goto end; + } + ///////////////////////////////////////////// + // to calculate map center, first set center as vessel position and + // trace path. get direction vector from vessel position and + // put map center 15nm ahead of vessel on that vector, and + // retrace map +printf("Route initial -- map center\n"); + load_world_5sec_map(ves_pos, path_map); + load_beacons_into_path_map(path_map); +//write_depth_map(path_map, "a.pnm"); + trace_route_simple(path_map, ves_pos); +//write_depth_map(path_map, "b.pnm"); +//write_path_map(path_map, "b.path.pnm"); + image_coordinate_type center = + { .x = MAP_LEVEL3_SIZE/2, .y = MAP_LEVEL3_SIZE/2 }; + rebuild_map_by_vessel_offset(path_map, center, ves_pos); + rc = 0; +end: + return rc; +} + From 4dd1e3fedbd7d7189395ff2e1617614bd11fb046 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 6 Apr 2022 00:42:21 -0700 Subject: [PATCH 10/30] Initial rework of path finding code complete. Some support functions still missing. Working on compiling. --- slam_sim/include/occupancy.hpp | 184 +++++++----- slam_sim/src/Makefile | 2 +- slam_sim/src/occupancy.cpp | 69 +++-- slam_sim/src/path_map.cpp | 346 +++++++++++++++++++++++ slam_sim/src/utils/path_map.cpp | 478 -------------------------------- 5 files changed, 506 insertions(+), 573 deletions(-) create mode 100644 slam_sim/src/path_map.cpp delete mode 100644 slam_sim/src/utils/path_map.cpp diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 6fb9e4c..b0e2c18 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -9,13 +9,19 @@ /*********************************************************************** -The occupancy grid here is a 2D map that's built form the output from +The occupancy grid here is a 2D map that's built from the output from a SLAM algorithm. It represents the obstacles observed from a bot's sensors and is used for the bot to navigate from one location to another using a pathfinding algorithm, a modified dijkstra pathfinding algorithm that incorporates additional features to better support exploration. -As elsewhere in the code, positive coordinates are to the up and right. +Two different coordinate schemes are used here. Real world coordinates +are increasing to the up and right, as elsewhere in the code. The map +itself is stored in an array which uses the standard image coordinate +system, with the first element in the array being at the top-left. +The code has to manage the switch between the two. +// TODO FIXME verify that this is indeed the case, and that the switch +// is being properly handled. ***********************************************************************/ #include @@ -30,88 +36,111 @@ As elsewhere in the code, positive coordinates are to the up and right. namespace slam_sim { -// Characteristics of a node in the map, including distance from this node -// to a/the destination. -// Occupied: how many times the node has been traveled through. -// Observed: how many times the node has been seen. -// Boundary: how many times the node had an obstruction detection in it. -struct map_node_t -{ - // Index of node that's one-closer to destination (-1 for no parent) - int32_t parent_idx; - // Direction toward destination. This is moving toward a node that's - // X-parents removed (e.g., 5) - float direction_degs; - - float occupied; - float observed; - float boundary; - - // Cost to traverse this node. This is computed as a function of the - // node's characteristics. - float traversal_cost; - - // Total cost to reach destination. This is a unitless measure that - // includes biases to avoid heavily trafficed areas to encourage - // exploration - float path_cost; +//////////////////////////////////////////////////////////////////////// +// Supporting types. X,Y values are used in many different contexts. To +// help avoid mixing paradigms, different types are defined for the +// different contexts. - void clear(); +// Physical location in world space. +struct world_coordinate_t +{ + float x_meters; + float y_meters; }; - -// Counter/flags for a node's characteristics when evaluating a sensor sweep. -struct map_node_flags_t +// Bounding area of map in world space. +struct map_size_t { - uint8_t occupied; - uint8_t observed; - uint8_t boundary; - - void clear(); + float x_meters; + float y_meters; }; - +// Dimensions of map (occupancy grid). struct grid_size_t { uint32_t rows; uint32_t cols; }; - -struct grid_coord_t +// Node location in grid. +struct grid_coordinate_t { uint32_t x; uint32_t y; }; - -struct world_coordinate_t +// Distance from one grid node to another, in units of grid coordinates. +struct node_offset_t { - float x_meters; - float y_meters; + int32_t dx; + int32_t dy; }; - -struct map_size_t +// Storage index of a node w/in the 2D grid. +struct grid_index_t { - float x_meters; - float y_meters; + uint32_t idx; }; -struct node_offset_t +//////////////////////////////////////////////////////////////////////// +// The map is composed of several nodes arranged on a grid. + +// Counter/flags for a node's characteristics when evaluating a sensor sweep. +// Keep these flags in a seperate structure to help prevent confusion w/ +// similarly named counters in map_node_t. +// Occupied: if the node has been traveled through. +// Observed: if the node has been seen. +// Boundary: if the node had an obstruction detection in it. +struct map_node_flags_t { - int32_t dx; - int32_t dy; + uint8_t occupied; + uint8_t observed; + uint8_t boundary; + uint8_t state; + + void clear(); }; -struct grid_index_t +#define PATH_NODE_FLAG_PROCESSED 0x01 +#define PATH_NODE_FLAG_IMPASSABLE 0x02 + +// Characteristics of a node in the map, including distance from this node +// to a/the destination. +// Occupied: how many times the node has been traveled through. +// Observed: how many times the node has been seen. +// Boundary: how many times the node had an obstruction detection in it. +struct map_node_t { - uint32_t idx; -}; + // Position of node w/in grid. + grid_coordinate_t pos; + + // Index of node that's one-closer to destination (-1 for no parent) + int32_t parent_idx; + // Direction toward destination. This is moving toward a node that's + // X-parents removed (e.g., 5) + float direction_degs; + + float occupied; + float observed; + float boundary; + + map_node_flags_t flags; + + // Cost to traverse this node. This is computed as a function of the + // node's characteristics. + float traversal_cost; + + // Total cost to reach destination. This is a unitless measure that + // includes biases to avoid heavily trafficed areas to encourage + // exploration + float path_cost; + void clear(); +}; +// The grid (map) itself. class occupancy_grid_t { public: @@ -121,13 +150,8 @@ class occupancy_grid_t occupancy_grid_t(gaia::slam::working_map_t&); ~occupancy_grid_t(); - / Initialization // Resets the map grid. void clear(); - // Initializes as inner map using outer map to set boundary conditions. - // This is meant to be used to build a higher resolution local map to use - // based on a larger lower resolution area map. - //void embed(const occupancy_grid_t& surrounding); // Returns a reference to tne map node at the specified location. map_node_t& get_node(float x_meters, float y_meters); @@ -138,8 +162,15 @@ class occupancy_grid_t void export_as_pnm(std::string file_name); + // Path finding. Traces all routes to the destination. If map is + // embedded in larger map, the larger map is used to set boundary + // conditions for this map. + void trace_routes(world_coordinate_t destination, + const occupancy_grid_t& parent_map); + void trace_routes(world_coordinate_t destination); + protected: - uint32_t get_node_index(float pos_x_meters, float pos_y_meters); + grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); void apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters); void apply_flags(); @@ -159,18 +190,41 @@ class occupancy_grid_t // this object, if >=0 then memory is owned by blob_cache. int32_t m_blob_id; map_node_t* m_grid; - map_node_flags_t* m_grid_flags; - //std::vector m_grid; - //std::vector m_grid_flags; - //////////////////////////////////////////// + world_coordinate_t m_destination; + + //////////////////////////////////////////////////////////////////// // Path-finding. This is an algorithm derived from D*, ported from // different project (used w/ permission). + // FIFO queue for collecting list of which nodes need to be processed + // and either be assigned a cost to reach destination or update that + // cost. It's incorrectly referred to as a 'stack' sometimes. std::queue m_queue; - void add_node_to_stack(map_node* root_node, grid_index_t root_idx, + + // Add node to queue for future processing. + void add_node_to_stack(map_node_t& root_node, grid_index_t root_idx, node_offset_t offset, const float traverse_wt = 1.0f); - world_coordinate_t m_destination; + // Check diagonal node to see if it should be added to queue, and + // do so if so. + void add_node_to_stack_diag(const map_node_t& root_node, + const grid_index_t root_idx, const node_offset_t offset, + const float traversal_cost); + + // Pop the next node off the stack and process it. + void process_next_stack_node(); + + // Returns approximate world location for this grid square. + world_coordinate_t get_node_location(const grid_coordinate_t& pos); + + // Trace route(s) to destination. To each node, assigns approximate + // vector to reach destination. + void compute_path_costs(); + + // Setting destination. Also used for anchoring a node to a particular + // value, such as when setting boundary conditions. + void add_anchor_to_path_stack(const grid_index_t idx, + const float path_weight); }; } // namespace slam_sim diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index f8989af..05d815c 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -10,7 +10,7 @@ endif TARGET = slam_sim -OBJS = main.o analyze.o occupancy.o rule_api.o +OBJS = main.o analyze.o occupancy.o rule_api.o path_map.o EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 2b42b79..fc052d1 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -39,11 +39,6 @@ using gaia::slam::area_map_t;; using gaia::slam::working_map_t;; -// During porting, pull in the file inline. Once it's cleaned up and -// compiles, merge the contents in here. -#include "utils/path_map.cpp" - - //////////////////////////////////////////////////////////////////////// // Constructors and destructors @@ -68,7 +63,6 @@ occupancy_grid_t::occupancy_grid_t(float node_width_meters, // Signal that this owns the memory allocation. m_blob_id = -1; m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); - m_grid_flags = (map_node_flags_t*) malloc(num_nodes * sizeof *m_grid_flags); clear(); } @@ -90,12 +84,10 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am) if (blob == NULL) { // Blob doesn't exist yet. Create it. - size_t sz = num_nodes * (sizeof *m_grid + sizeof *m_grid_flags); + size_t sz = num_nodes * sizeof *m_grid; blob = blob_cache_t::get()->create_or_reset_blob(am.blob_id(), sz); } m_grid = (map_node_t*) blob->data; - size_t flag_offset = num_nodes * sizeof *m_grid_flags; - m_grid_flags = (map_node_flags_t*) &blob->data[flag_offset]; clear(); } @@ -117,12 +109,10 @@ occupancy_grid_t::occupancy_grid_t(working_map_t& wm) if (blob == NULL) { // Blob doesn't exist yet. Create it. - size_t sz = num_nodes * (sizeof *m_grid + sizeof *m_grid_flags); + size_t sz = num_nodes * sizeof *m_grid; blob = blob_cache_t::get()->create_or_reset_blob(wm.blob_id(), sz); } m_grid = (map_node_t*) blob->data; - size_t flag_offset = num_nodes * sizeof *m_grid_flags; - m_grid_flags = (map_node_flags_t*) &blob->data[flag_offset]; clear(); } @@ -133,7 +123,6 @@ occupancy_grid_t::~occupancy_grid_t() { // Memory is owned by this. free(m_grid); - free(m_grid_flags); } } @@ -150,8 +139,10 @@ void map_node_t::clear() this->observed = 0.0f; this->boundary = 0.0f; - this->traversal_cost = 0.0f; - this->distance = 0.0f; + this->flags.clear(); + + this->traversal_cost = 1.0f; + this->path_cost = 0.0f; } @@ -160,19 +151,21 @@ void map_node_flags_t::clear() this->occupied = 0; this->observed = 0; this->boundary = 0; + this->state = 0; } void occupancy_grid_t::clear() { - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - for (uint32_t i=0; i 0.0) + { + // Make an exception if it's rarely observed, as it could have + // been a measurement error. + if (flags.boundary != 0) + { + // Boundary was just seen. Observe it regardless of any + // flukes in the past. + node.flags.state |= PATH_NODE_FLAG_IMPASSABLE; + } + else if (node.boundary / node.observed > 0.1) + { + assert(node.observed > 0.0); + node.flags.state |= PATH_NODE_FLAG_IMPASSABLE; + } + } } } diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp new file mode 100644 index 0000000..288dabd --- /dev/null +++ b/slam_sim/src/path_map.cpp @@ -0,0 +1,346 @@ +/*********************************************************************** +* Copyright (C) 2019-2022 Keith Godfrey +* Copyright (C) 2022 Gaia +* +* Use of this source code is governed by the MIT +* license that can be found in the LICENSE.txt file +* or at https://opensource.org/licenses/MIT. +***********************************************************************/ + +/*********************************************************************** +* Path-finding logic for occupancy grid. Code here adapted from +* <> +***********************************************************************/ + +#include +#include +#include +#include + +#include "occupancy.hpp" + + +// If node at root+offset belongs as part of path, sets node values +// (eg, weight and link to parent) +// Add node to stack for future neighbor analysis. +// 'traversal_cost' is the cost to reach this node from the neighbor that +// added it to the stack. +void occupancy_grid_t::add_node_to_stack( + /* in */ const map_node_t& root_node, + /* in */ const grid_index_t root_idx, + /* in */ const node_offset_t offset, + /* in */ const float traversal_cost + ) +{ + // Make sure we're not going off the edge of the map. If so, ignore. + int32_t new_x = (int32_t) root_node.pos.x + offset.dx; + int32_t new_y = (int32_t) root_node.pos.y + offset.dy; + if ((new_x < 0) || (new_x >= m_grid_size.cols) || (new_y < 0) || + (new_y >= m_grid_size.rows)) + { + return; + } + ///////////////////////////////////////////////////////////////////// + // Point is in the world. Check it. + uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); + map_node_t& child_node = grid->nodes[new_idx]; + if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) + { + return; + } + // Determine weight for traversing this node. Have lower weight to nodes + // infrequently observed/traversed to provide bias to take different + // routes. + float weight = 1.0f; + weight += child_node.observed * c_path_penalty_per_observation; + // Path tracing algorithm is deterministic and can follow vert or + // horiz path too easily. Add some jitter to allow pulling in + // from more accurate direction. + double jitter = 0.0; + drand48_r(&path_rand_, &jitter); + jitter = 0.1 * (jitter - 0.5); + float new_cost = root_node.path_cost + penalty + traversal_wt + jitter; + if (child_node->flags.state & PATH_NODE_FLAG_PROCESSED) { + // This node is already-processed. See if this path might provide + // it a lower cost. + if (child_node->path_cost <= new_cost) + { + // Nope. + return; + } + // New weight is lower -- allow node to be added to stack again to + // propagate updated weight to neighbors. + } + // Add point to stack for future consideration. + child_node->parent_id = root_idx; + child_node->weight = new_weight; + child_node->traversal_cost = penalty; + child_node->flags.state |= PATH_NODE_FLAG_PROCESSED; + m_queue.push(new_idx); +} + + +// if node at root+offset belongs as part of path, sets node values +// (eg, weight and link to parent) +// for node that's diagonal, include weight of 4-connected neighbor required +// to reach diagonal node +// add pixel to stack for future neighbor analysis +void occupancy_grid_t::add_node_to_stack_diag( + /* in */ const map_node_t& root_node, + /* in */ const grid_index_t root_idx, + /* in */ const node_offset_t offset + ) +{ + // make sure we're not going off the edge of the map + int32_t new_x = (int32_t) root_node.pos.x + offset.dx; + int32_t new_y = (int32_t) root_node.pos.y + offset.dy; + if ((new_x < 0) || (new_x >= m_grid_size.cols) + || (new_y < 0) || (new_y >= m_grid_size.rows)) + { + return; + } + ///////////////////////////////////////////////////////////////////// + // point is in the world -- check it + uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); + map_node_t& child_node = m_grid[new_idx]; + if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) + { + return; + } + + // see if there's a way to get to this diagonal node + // check via vertical and horizontal neighbor + uint32_t idx_vert = + (uint32_t) (root_node.pos.x + new_y * m_grid_size.cols); + uint32_t idx_horiz = + (uint32_t) (new_x + root_node.pos.y * m_grid_size.cols); + map_node_t& vert_child_node = m_grid[idx_vert]; + map_node_t& horiz_child_node = m_grid[idx_horiz]; + if ((vert_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) && + (horiz_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE)) + { + // no direct 4-connected path. nothing to do here + return; + } + ///////////////////////////////////////////////////////////////////// + // Point is reachable. Check cost to get there. This will be + // combination of cost from easiest 4-connected path to get + // to diagonal node + // and of diagonal node itself + float cost = -1.0f; + // get lowest traversal weight of vert and horiz paths and use that as + // base for weight to diagonal node + if ((vert_child_node->flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { + cost = vert_child_node->traversal_cost; + } + if ((horiz_child_node->flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { + float h_cost = horiz_child_node->traversal_cost; + if (cost < 0.0f) { + // Not accessible by vert node but horiz provides path. Use + // horiz penalty. + cost = h_cost; + } else if ((h_cost > 0.0f) && (h_cost < cost)) { + // Horiz node is passable and penalty is less than via vert, so + // use Horiz's penalty + cost = penalty; + } + } + assert(cost >= 0.0); + // Cost needed to traverse to neighboring node. Scale it up because + // distance is farther (it's on a diagonal). + float traversal_weight = cost * 1.41f; + add_node_to_stack(root_node, root_idx, offset, traversal_weight); +end: + ; +} + +// pixel neighbors +// 4-connected base +static const node_offset_t OFF_E = { .dx= 1, .dy= 0 }; +static const node_offset_t OFF_W = { .dx=-1, .dy= 0 }; +static const node_offset_t OFF_N = { .dx= 0, .dy=-1 }; +static const node_offset_t OFF_S = { .dx= 0, .dy= 1 }; +// 8-connected extras +static const node_offset_t OFF_NE = { .dx= 1, .dy=-1 }; +static const node_offset_t OFF_NW = { .dx=-1, .dy=-1 }; +static const node_offset_t OFF_SE = { .dx= 1, .dy= 1 }; +static const node_offset_t OFF_SW = { .dx=-1, .dy= 1 }; + + +// pop the next node off the stack and processess it +static void occupancy_grod_t::process_next_stack_node() +{ + // Add 4-connected nodes to stack. + grid_index_t root_idx = m_queue.pop(); + map_node_t& root_node = m_grid[root_idx.idx]; + // + add_node_to_stack(root_node, root_idx, OFF_E); + add_node_to_stack(root_node, root_idx, OFF_W); + add_node_to_stack(root_node, root_idx, OFF_N); + add_node_to_stack(root_node, root_idx, OFF_S); + // Add 8-connected nodes to stack. This will only happen if there's + // a valid 4-connected path to get there. + add_node_to_stack_diag(root_node, root_idx, OFF_NE); + add_node_to_stack_diag(root_node, root_idx, OFF_NW); + add_node_to_stack_diag(root_node, root_idx, OFF_SE); + add_node_to_stack_diag(root_node, root_idx, OFF_SW); +} + + +// Use D* approach to find all routes to destination +// Also builds vector of approximate course to follow for all nodes in path map. +// Present algorithm is very simple and is based on direction to 'grandparent' +// node, approx. 5 'generations' away. +void occupancy_grid_t::compute_path_costs() +{ + // Update path weight between nodes until stack is empty. + while (m_queue.size() > 0) + { + process_next_stack_node(); + } + // Now follow gradient in each node to build directional vector + // to reach destination. + pixel_offset_bitfield_type base_direction, next_direction; + // Iterate through nodes and build direction vector for each. Vector + // is defined by following the path toward the destination (i.e., + // stepping between adjacent nodes) out by several nodes, and measuing + // the bearing to that node. If the path turns sharply (e.g., around + // an obstacle) then the path is only traced to the turning point. + for (uint32_t y=0; ynodes[idx]; + map_node_t& ggp = root; + // Find direction to ancestor. + for (uint32_t gen=0; gen= 0) + { + map_node_t& next_ggp = path_map->nodes[ggp.parent_id.idx]; + if (gen == 0) + { + // Get base direction. This returns bitfield + // indicating direction of next node relative + // to this one. + base_direction = get_offset_mask(next_ggp.pos, ggp.pos); + } + else + { + // Make sure offset is consistent w/ base direction + // (ie, w/in 45deg). + // Get new direction. This returns bitfield indicating + // direction of next node relative to this one, + // plus adjacent bits. ANDing this to base direction + // will indicate if it's w/in +/-45deg of base. + next_direction = + get_offset_mask_wide(next_ggp.pos, ggp.pos); + if ((next_direction.mask & base_direction.mask) == 0) + { + // Latest direction is too different from original + // offset. Halt search. + break; + } + } + ggp = next_ggp; + } else { + break; + } + } + path_offset_type dir; + float dx = (float) (ggp->pos.x - root->pos.x); + float dy = (float) (ggp->pos.y - root->pos.y); + float theta_degs = R2D * atan2f(dx, -dir.dy); + if (theta_degs < 0.0) + { + theta_degs += 360.0; + } + root.direction_degs = theta_degs; + } + } +} + + +// Sets map node to specific weight. This can be a destination node (e.g., +// weight=0.0) or a map's boundary conditions (e.g., if embedding a high +// res map w/in a low res). +void occupancy_grid_t::add_anchor_to_path_stack( + /* in */ const grid_index_t idx, + /* in */ const float path_weight + ) +{ + assert(idx.idx < m_grid_size.cols * m_grid_size.rows); + map_node_t& path_node = m_grid[idx.idx]; + path_node.weight = path_weight; + path_node.parent_id.val = -1; + m_queue.push(idx); + path_node->flags.state = PATH_NODE_FLAG_PROCESSED; +} + + +world_coordinate_t occupancy_grid_t::get_node_location( + const grid_coordinate_t& pos) +{ + world_coordinate_t world_pos; + assert(pos.x < m_grid_size.cols); + assert(pos.y < m_grid_size.rows); + world_pos.x_meters = m_bottom_left.x_meters + + ((float) pos.x + 0.5) * m_node_size_meters; + world_pos.y_meters = m_bottom_left.y_meters + m_map_size.y_meters - + ((float) pos.y + 0.5) * m_node_size_meters; + return world_pos; +} + + +// Builds map to destination using parent map to set boundary conditions. +// If destination is outside of map bounds, only parent map is used +// for path finding. +void occupancy_grid_t::trace_routes(world_coordinate_t destination, + const occupancy_grid_t& parent_map) +{ + clear(); + // Add anchors. First destination, then boundary conditions. + grid_index_t idx = get_node_index(destination.x_meters, + destination.y_meters; + add_anchor_to_path_stack(idx, 0.0f); + // For each boundary point, get distance/weight from parent map at + // that location and use that value to set an anchor here. + for (uint32_t y=0; y 0) && (y < m_grid_size.rows)) + { + // In this row, anchor first and last columns only + grid_coordinate_t pos = { .x=x, .y=y }; + world_coordinate_t world_pos = get_node_location(pos); + // It's assumed that this map is a subset of parent map and + // is fully contained in it as that's a design constraint. + map_node_t& parent_node = parent_map.get_node(world_pos.x_meters, + world_pos.y_meters); + add_anchor_to_path_stack(pos, parent_node.path_cost); + } + else + { + // This is a top or bottom row. Anchor all nodes in this row. + for (uint32_t x=0; x -#include -#include -#include - -#include "occupancy.hpp" - - -static bool is_not_passable(map_node_t& child_node) -{ - if (child_node.boundary > 0.0) - { - // A boundary was observed in this node. If it's not a fluke, - // it's not passable. - assert(child_node.observed > 0.0); - if (child_node.boundary / child_node.observed > 0.1) - { - return; - } - } -} - - -// if node at root+offset belongs as part of path, sets node values -// (eg, weight and link to parent) -// add pixel to stack for future neighbor analysis -// 'distance' is the cost to reach this node from a neighbor (i.e., 1.0 or -// 1.4, depending if it's NSEW or diagonal neighbor) -void occupancy_grid_t::add_node_to_stack( - /* in */ const map_node_t& root_node, - /* in */ const grid_index_t root_idx, - /* in */ const node_offset_t offset, - /* in */ const float traversal_wt - ) -{ - // make sure we're not going off the edge of the map - int32_t new_x = (int32_t) root_node.pos.x + offset.dx; - int32_t new_y = (int32_t) root_node.pos.y + offset.dy; - if ((new_x < 0) || (new_x >= m_grid_size.cols) || (new_y < 0) || - (new_y >= m_grid_size.rows)) - { - return; - } - ///////////////////////////////////////////////////////////////////// - // point is in the world -- check it - uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); - map_node_t& child_node = grid->nodes[new_idx]; - if (is_not_passable(child_node)) - { - return; - } - // Determine weight for traversing this node. Have lower weight to nodes - // infrequently observed/traversed to provide bias to take different - // routes. - float weight = 1.0f; - weight += child_node.observed * c_path_penalty_per_observation; - // Path tracing algorithm is deterministic and can follow vert or - // horiz path too easily. Add some jitter to allow pulling in - // from more accurate direction. - double jitter = 0.0; - drand48_r(&path_rand_, &jitter); - jitter = 0.1 * (jitter - 0.5); - - float new_weight = root_node.weight + penalty + traversal_wt - + (float) jitter; - if (child_node->flags & PATH_NODE_FLAG_PROCESSED) { - // already-processed node - // see if this path might provide it a lower weight - if (child_node->weight <= new_weight) - { - // Nope. - return; - } - // new weight is lower -- allow to be added to stack again to propagate - // updated weight to neighbors - } - // add point to stack for - child_node->parent_id = root_idx; - child_node->weight = new_weight; - child_node->passage_penalty = penalty; - child_node->flags |= PATH_NODE_FLAG_PROCESSED; - // - m_queue.push(new_idx); -end: - ; -} - - -// if node at root+offset belongs as part of path, sets node values -// (eg, weight and link to parent) -// for node that's diagonal, include weight of 4-connected neighbor required -// to reach diagonal node -// add pixel to stack for future neighbor analysis -void occupancy_grid_t::add_node_to_stack_diag( - /* in */ const map_node_t& root_node, - /* in */ const grid_index_t root_idx, - /* in */ const node_offset_t offset - ) -{ - // make sure we're not going off the edge of the map - int32_t new_x = (int32_t) root_node.pos.x + offset.dx; - int32_t new_y = (int32_t) root_node.pos.y + offset.dy; - if ((new_x < 0) || (new_x >= m_grid_size.cols) - || (new_y < 0) || (new_y >= m_grid_size.rows)) - { - goto end; - } - ///////////////////////////////////////////////////////////////////// - // point is in the world -- check it - uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); - map_node_t& child_node = m_grid[new_idx]; - if (is_not_passable(child_node)) - { - return; - } - - // see if there's a way to get to this diagonal node - // check via vertical and horizontal neighbor - uint32_t idx_vert = - (uint32_t) (root_node.pos.x + new_y * m_grid_size.cols); - uint32_t idx_horiz = - (uint32_t) (new_x + root_node.pos.y * m_grid_size.cols); - map_node_t *vert_child_node = &m_grid[idx_vert]; - map_node_t *horiz_child_node = &m_grid[idx_horiz]; - - if ((vert_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) && - (horiz_child_node->flags & PATH_NODE_FLAG_NO_ACCESS)) { - // no direct 4-connected path. nothing to do here - goto end; - } - ///////////////////////////////////////////////////////////////////// - // point is reachable. check weight. this will be combination of - // weight from easiest 4-connected path to get to diagonal node - // and of diagonal node itself - float nbr_penalty = -1.0f; - // get lowest traversal weight of vert and horiz paths and use that as - // base for weight to diagonal node - if ((vert_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) == 0) { - nbr_penalty = vert_child_node->passage_penalty; - } - if ((horiz_child_node->flags & PATH_NODE_FLAG_NO_ACCESS) == 0) { - float penalty = horiz_child_node->passage_penalty; - if (nbr_penalty < 0.0f) { - // not accessible by vert node, but horiz provides path. use - // horiz penalty - nbr_penalty = penalty; - } else if ((penalty > 0.0f) && (penalty < nbr_penalty)) { - // horiz node is passable and penalty is less than via vert, so - // use horiz's penalty - nbr_penalty = penalty; - } - } - if (nbr_penalty < 0.0f) { - // no direct 4-connected path. nothing to do here - goto end; - } - // weight needed to traverse neighboring node - // diagonal weight should be 1.414, but there appears to be a strong - // bias for horiz/vert travel. lower weight to give diagonal more - // preference, to balance things out - float traversal_weight = nbr_penalty + 1.25f; - add_node_to_stack(root_node, root_idx, offset, traversal_weight); -end: - ; -} - -// pixel neighbors -// 4-connected base -static const node_offset_t OFF_E = { .dx= 1, .dy= 0 }; -static const node_offset_t OFF_W = { .dx=-1, .dy= 0 }; -static const node_offset_t OFF_N = { .dx= 0, .dy=-1 }; -static const node_offset_t OFF_S = { .dx= 0, .dy= 1 }; -// 8-connected extras -static const node_offset_t OFF_NE = { .dx= 1, .dy=-1 }; -static const node_offset_t OFF_NW = { .dx=-1, .dy=-1 }; -static const node_offset_t OFF_SE = { .dx= 1, .dy= 1 }; -static const node_offset_t OFF_SW = { .dx=-1, .dy= 1 }; - - -// pop the next node off the stack and processess it -static void occupancy_grod_t::process_next_stack_node() -{ - // add adjacent nodes to stack - grid_index_t root_idx = m_queue.pop(); - map_node_t& root_node = m_grid[root_idx.idx]; - // - add_node_to_stack(root_node, root_idx, OFF_E); - add_node_to_stack(root_node, root_idx, OFF_W); - add_node_to_stack(root_node, root_idx, OFF_N); - add_node_to_stack(root_node, root_idx, OFF_S); - // add 8-connected nodes if there's a valid 2-step 4-conneced path - // to get there - add_node_to_stack_diag(root_node, root_idx, OFF_NE); - add_node_to_stack_diag(root_node, root_idx, OFF_NW); - add_node_to_stack_diag(root_node, root_idx, OFF_SE); - add_node_to_stack_diag(root_node, root_idx, OFF_SW); -} - - -// build vector of approximate course to follow for all nodes in path map. -// present algorithm is very simple and is based on direction to 'grandparent' -// node, approx. 5 'generations' away -// TODO present implementation gives very poor angular accuracy and misses -// turns, potentially aiming it toward land or across shallow water. -// route calculation should compensate for this but it should be fixed -// here at the source. build more accurate direction vectors for each -// path map node -// TODO migrate away from vectors and only calculate course -// also calculates course from vector -// TODO re-evaluate and fix for use in arctic ocean -static void build_course_vectors( - /* in out */ occupancy_grid_t *path_map, - /* in */ const degree_type center_latitude - ) -{ - image_size_type size = path_map->size; - // build direction vector for each node - double dy_deg = 1.0 / (double) size.y; - double y_deg_top = center_latitude.degrees - dy_deg * (double) (size.y / 2); - pixel_offset_bitfield_type base_direction, next_direction; - // - for (uint32_t y=0; ynodes[idx]; - map_node_t *ggp = root; -//printf("%d,%d scale=%.4f\n", root->pos.x, root->pos.y, scale); - // find direction to ancestor - for (uint32_t gen=0; genparent_id.val >= 0) { - map_node_t *next_ggp = - &path_map->nodes[ggp->parent_id.idx]; - if (gen == 0) { - // get base direction. this returns bitfield indicating - // direction of next node relative to this one - base_direction = get_offset_mask(next_ggp->pos, ggp->pos); - } else { - // make sure offset is consistent w/ base direction - // (ie, w/in 45deg) - // get new direction. this returns bitfield indicating - // direction of next node relative to this one, - // plus adjacent bits. ANDing this to base direction - // will indicate if it's w/in +/-45deg of base - next_direction = - get_offset_mask_wide(next_ggp->pos, ggp->pos); - if ((next_direction.mask & base_direction.mask) == 0) { - // latest direction is too different from original - // offset. halt search - break; - } - } - ggp = next_ggp; -//printf(" ->%d,%d %.3f\n", ggp->pos.x, ggp->pos.y, ggp->passage_penalty); - } else { - break; - } - } - path_offset_type dir; - dir.dx = (int16_t) (ggp->pos.x - root->pos.x); - dir.dy = (int16_t) (ggp->pos.y - root->pos.y); - double theta_deg = R2D * atan2((double) dir.dx * scale, (double) -dir.dy); -//printf(" = %d,%d for %.3f (x-scale= %.3f, wt=%.3f)\n", dir.dx, dir.dy, theta_deg, scale, ggp->weight); - CVT_DEG_TO_BAM16(theta_deg, root->true_course); - // don't need to reset active course -- it's set when smoothing - // out course - // active course is reset when smoothing course, but not for all - // all nodes (ie, boundary nodes). take care of those cases here - root->active_course = root->true_course; -//printf("INIT %d %d,%d [%d] dir %d,%d %.2f (active %.2f)\n", idx, x, y, root->parent_id.val, dir.dx, dir.dy, (double) root->true_course.angle16 * BAM16_TO_DEG, root->active_course.angle16 * BAM16_TO_DEG); - } - } -} - - -// adds point (ie, destination or beacon) to path map, using specified -// path weight -void occupancy_grid_t::add_destination_to_path_stack( - /* in */ const image_coordinate_type pos, - /* in */ const float path_weight - ) -{ - // make sure pixel is in map - if ((pos.x < MAP_LEVEL3_SIZE) || (pos.y < MAP_LEVEL3_SIZE)) { - uint32_t idx = (uint32_t) (pos.x + pos.y * MAP_LEVEL3_SIZE); - map_node_t* path_node = m_grid[idx]; - path_node.weight = path_weight; - path_node.parent_id.val = -1; - m_queue.push(idx); - path_node->flags = PATH_NODE_FLAG_PROCESSED; -//fprintf(stderr, "Seeded %d,%d with %.1f\n", pos.x, pos.y, (double) path_weight); - } -} - -// use D* approach to find all routes to destination -// path traced on existing depth map, using beacons and destination as -// seed locations -void trace_route_simple( - /* out */ occupancy_grid_t *path_map, - /* in */ const world_coordinate_type vessel_pos - ) -{ -//printf("TRACE SIMPLE %d beacons\n", path_map->num_beacons); - reset_path_map(path_map); -// path_map->destination = convert_latlon_to_akn(path_map->center); - // add beacon and dest weights to map, as able - path_map->read_idx = 0; - path_map->write_idx = 0; - // add destination as primary seed with 0 weight - // if destination is beyond visible map then it will be filtered and - // not actually added stack - calculate_destination_map_position(path_map); - add_point_to_path_stack(path_map, path_map->dest_pix, 0.0f); - // add beacons with seeds of their computed path weights to destination - for (uint32_t i=0; inum_beacons; i++) { - // TODO don't add beacon if w/in X distance of vessel. that's too - // close and likely to result in artifacts - map_beacon_reference_type *ref = &path_map->beacon_ref[i]; - world_coordinate_type beac_pos = convert_akn_to_world(ref->coords); - meter_type dist = calc_distance(vessel_pos, beac_pos, __func__); - if (dist.meters < VESSEL_BEACON_INHIBITION_RING_NM * NM_TO_METERS) { - // beacon is too close to vessel. inhibit its placement in map - continue; - } - float weight = path_map->beacon_ref[i].path_weight; - if (weight > 0.0f) { -//printf(" adding beacon %d to stack (wt=%.1f)\n", path_map->beacon_ref[i].index, (double) weight); - // if using beacon weights directly, variations in paths can - // result in a beacon being a local minimum that the path - // cannot escape from. to fix this problem, multiply all - // beacon weights by a constant (e.g., 2x). this way the - // the lowest weight beacon in a map will be the one that - // has the strongest attraction, as it should overcome the - // weight of all beacons between it and the vessel. this way - // we can avoid the local minimum problem without complicated - // logic that determines the best beacon to drive toward, and - // which are of lower priority (ie, are closer) while covering - // for all terrain variations - add_point_to_path_stack(path_map, - path_map->beacon_ref[i].pos_in_map, 2.0f * weight); - } -//else { printf(" beacon %d has wt=%.1f\n", path_map->beacon_ref[i].index, weight); } - } - // update path weight between nodes until stack is empty - uint32_t num_nodes = MAP_LEVEL3_SIZE * MAP_LEVEL3_SIZE; -//uint32_t cnt = 0; - while (path_map->read_idx < path_map->write_idx) { -//cnt++; - // because nodes can be added to the stack multiple times it's - // possible for stack to grow beyond initial size (which is - // number of nodes). if stack overflows, purge used contents - // and shrink it - if (path_map->write_idx >= num_nodes) { - shrink_path_stack(path_map); - } - process_next_stack_node(path_map); - } -//printf("Processed %d nodes\n", cnt); - degree_type center_latitude = { .degrees = path_map->center.latitude }; - build_course_vectors(path_map, center_latitude); -} - - -// generate map based on existing map, with map center being X-nm in -// front of vessel with vessel following existing path map's route -// to get to destination -void rebuild_map_by_vessel_offset( - /* in out */ occupancy_grid_t *path_map, - /* in */ const image_coordinate_type vessel_pix, - /* in */ const world_coordinate_type vessel_pos - ) -{ -printf("Rebuild map by vessel offset\n"); - uint32_t idx = (uint32_t) (vessel_pix.x + vessel_pix.y * path_map->size.x); - map_node_t *node = &path_map->nodes[idx]; - // if node doesn't have direction information, that's an error, but - // tolerate it. can't offset if this node has no path info, so - // center map on vessel - if (node->weight < 0.0f) { - // can't generate offset if vessel's node has no path info, so just - // center map on vessel -fprintf(stderr, "Generating offset map, but vessel pix %d,%d in old map has no path info. Vessel at %.4f,%.4f\n", vessel_pix.x, vessel_pix.y, vessel_pos.lon, vessel_pos.lat); - log_err(log_, "Generating offset map, but vessel pix %d,%d in old " - "map has no path info. Vessel at %.4f,%.4f", - vessel_pix.x, vessel_pix.y, vessel_pos.lon, vessel_pos.lat); - // -printf(" (no path info)\n"); - load_world_5sec_map(vessel_pos, path_map); - } else { - // get direction vessel should be moving and build map based on that - // direction to head to reach destination - degree_type course = { .degrees = - (double) node->true_course.angle16 * BAM16_TO_DEG }; - // calc where map center should be, in that direction - meter_type dist = - { .meters = VESSEL_OFFSET_FROM_MAP_CENTER_NM * NM_TO_METERS }; - world_coordinate_type new_center = - calc_offset_position(vessel_pos, course, dist); - // load map - load_world_5sec_map(new_center, path_map); - } - load_beacons_into_path_map(path_map); -//write_depth_map(path_map, "c.pnm"); - trace_route_simple(path_map, vessel_pos); - path_map->vessel_start_pix = get_pix_position_in_map(path_map, - vessel_pos); -//write_depth_map(path_map, "d.pnm"); -} - - -// initial beacon trace for route plus determining appropriate world -// map center -// performs several traces. first beacons are traced. then trace is -// made with vessel at center, then map is moved and trace is made -// with vessel offset X-nm from center (e.g., 15nm) and direction -// of travel moving through center -// returns 0 on success and -1 on failure -int trace_route_initial( - /* in out */ occupancy_grid_t *path_map, - /* in */ const world_coordinate_type dest, - /* in */ const world_coordinate_type vessel_pos - ) -{ - // TODO either use constants or use size. right now these are - // unreasonably intermixed - assert(path_map->size.x == MAP_LEVEL3_SIZE); - assert(path_map->size.y == MAP_LEVEL3_SIZE); - //////////////// - int rc = -1; - log_info(log_, "Computing routes from %.5f,%.5f to %.5f,%.5f", - vessel_pos.x_deg, vessel_pos.y_deg, dest.x_deg, dest.y_deg); - world_coordinate_type destination = dest; - if (destination.lon < 0.0) { - destination.lon += 360.0; - } - check_world_coordinate(destination, __func__); - world_coordinate_type ves_pos = vessel_pos; - if (ves_pos.lon < 0.0) { - ves_pos.lon += 360.0; - } - check_world_coordinate(ves_pos, __func__); - path_map->destination = convert_latlon_to_akn(destination); - // construct beacon path map - if (trace_beacon_paths(path_map, dest) != 0) { - // something bad happened. error should have been reported from - // w/in function itself - goto end; - } - ///////////////////////////////////////////// - // to calculate map center, first set center as vessel position and - // trace path. get direction vector from vessel position and - // put map center 15nm ahead of vessel on that vector, and - // retrace map -printf("Route initial -- map center\n"); - load_world_5sec_map(ves_pos, path_map); - load_beacons_into_path_map(path_map); -//write_depth_map(path_map, "a.pnm"); - trace_route_simple(path_map, ves_pos); -//write_depth_map(path_map, "b.pnm"); -//write_path_map(path_map, "b.path.pnm"); - image_coordinate_type center = - { .x = MAP_LEVEL3_SIZE/2, .y = MAP_LEVEL3_SIZE/2 }; - rebuild_map_by_vessel_offset(path_map, center, ves_pos); - rc = 0; -end: - return rc; -} - From 7c58c4fe6b3c7d00dc2b21eb37f1ba975170ead9 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 6 Apr 2022 11:41:34 -0700 Subject: [PATCH 11/30] Compiles after incorporating path finding. Now to link. --- slam_sim/include/constants.hpp | 7 + slam_sim/include/occupancy.hpp | 18 +-- slam_sim/src/occupancy.cpp | 2 +- slam_sim/src/path_map.cpp | 240 +++++++++++++++++++++++++-------- 4 files changed, 203 insertions(+), 64 deletions(-) diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 29a5f9f..c2615a6 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -1,4 +1,5 @@ #pragma once +#include namespace slam_sim { @@ -25,6 +26,12 @@ constexpr float c_standard_map_node_width_meters = 0.05f; // in cost to traverse a node for each time it's been observed. constexpr float c_path_penalty_per_observation = 0.05f; +// The Dijkstra-like path finding algorithms are very rough on the local +// scale, having at most 8 directions to move away from a node in the map. +// The approach here to smooth that out is to look at the path necessary +// to trace out X nodes from a given location and use that to generate +// the directional vector from each node. The constant here is that X. +constexpr uint32_t c_num_ancestors_for_direction = 5; } // namespace slam_sim diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index b0e2c18..387d2a3 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -79,8 +79,11 @@ struct node_offset_t // Storage index of a node w/in the 2D grid. struct grid_index_t { + // The index here is unsigned in order to help detect improper usage + // (e.g., index of -1). uint32_t idx; }; +constexpr uint32_t c_invalid_grid_idx = 0xffffffff; //////////////////////////////////////////////////////////////////////// @@ -116,7 +119,7 @@ struct map_node_t grid_coordinate_t pos; // Index of node that's one-closer to destination (-1 for no parent) - int32_t parent_idx; + grid_index_t parent_idx; // Direction toward destination. This is moving toward a node that's // X-parents removed (e.g., 5) float direction_degs; @@ -166,7 +169,7 @@ class occupancy_grid_t // embedded in larger map, the larger map is used to set boundary // conditions for this map. void trace_routes(world_coordinate_t destination, - const occupancy_grid_t& parent_map); + occupancy_grid_t& parent_map); void trace_routes(world_coordinate_t destination); protected: @@ -199,17 +202,16 @@ class occupancy_grid_t // FIFO queue for collecting list of which nodes need to be processed // and either be assigned a cost to reach destination or update that // cost. It's incorrectly referred to as a 'stack' sometimes. - std::queue m_queue; + std::queue m_queue; // Add node to queue for future processing. - void add_node_to_stack(map_node_t& root_node, grid_index_t root_idx, - node_offset_t offset, const float traverse_wt = 1.0f); + void add_node_to_stack(const grid_index_t root_idx, + const node_offset_t offset, const float traverse_wt = 1.0f); // Check diagonal node to see if it should be added to queue, and // do so if so. - void add_node_to_stack_diag(const map_node_t& root_node, - const grid_index_t root_idx, const node_offset_t offset, - const float traversal_cost); + void add_node_to_stack_diag(const grid_index_t root_idx, + const node_offset_t offset); // Pop the next node off the stack and process it. void process_next_stack_node(); diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index fc052d1..5532a1f 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -132,7 +132,7 @@ occupancy_grid_t::~occupancy_grid_t() void map_node_t::clear() { - this->parent_idx = -1; + this->parent_idx.idx = c_invalid_grid_idx; this->direction_degs = 0.0f; this->occupied = 0.0f; diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 288dabd..870bdf6 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -12,13 +12,134 @@ * <> ***********************************************************************/ +#include +#include #include #include -#include #include +#include "constants.hpp" #include "occupancy.hpp" +namespace slam_sim +{ + +//////////////////////////////////////////////////////////////////////// +// Supporting API + +// Offset between two adjacent pixels represented as a bitfield. +// 0x80 is north, 0x40 is NE, etc (see below). +// The use case here is to determine if the offset from pixels A0-B0 +// is the same as the offset from pixels A1-B1. +// The bitfield is "blurred" to determine if two offsets are similar, +// with offset X being, e.g., 0b00100000, being blurred to 0b01110000, +// so if offset Y is ANDed to this, we can quickly determine if X and +// Y are similar (i.e., if Y is w/in 45 degrees of X). +struct pixel_offset_bitfield_t { + uint8_t mask; +}; +typedef struct pixel_offset_bitfield pixel_offset_bitfield_type; + + +// given two adjacent pixels, generate bitfield indicating which direction +// b is from a. one bit is set for either of the 8 possibile offsets +static pixel_offset_bitfield_t get_offset_mask( + /* in */ const grid_coordinate_t a, + /* in */ const grid_coordinate_t b + ) +{ + pixel_offset_bitfield_t offset; + int32_t dx = a.x - b.x + 1; + int32_t dy = a.y - b.y + 1; + assert(dx <= 2); + assert(dy <= 2); + uint32_t idx = (uint32_t) (dx + dy * 3); + switch (idx) { + case 1: // N + offset.mask = 0b10000000; // 0x80 + break; + case 2: // NE + offset.mask = 0b01000000; // 0x40 + break; + case 5: // E + offset.mask = 0b00100000; // 0x20 + break; + case 8: // SE + offset.mask = 0b00010000; // 0x10 + break; + case 7: // S + offset.mask = 0b00001000; // 0x08 + break; + case 6: // SW + offset.mask = 0b00000100; // 0x04 + break; + case 3: // W + offset.mask = 0b00000010; // 0x02 + break; + case 0: // NW + offset.mask = 0b00000001; // 0x01 + break; + case 4: // no offset -- fall through + default: // not reachable (no legal default) + offset.mask = 0; + break; + } + return offset; +} + +// Given two adjacent pixels, generate bitfield indicating which direction +// b is from a. Three bits are set for either of the 8 possibile offsets, +// with the center of that field matching the above get_offset_mask(). +static pixel_offset_bitfield_t get_offset_mask_wide( + /* in */ const grid_coordinate_t a, + /* in */ const grid_coordinate_t b + ) +{ + pixel_offset_bitfield_t offset; + int32_t dx = a.x - b.x + 1; + int32_t dy = a.y - b.y + 1; + assert(dx <= 2); + assert(dy <= 2); + uint32_t idx = (uint32_t) (dx + dy * 3); + switch (idx) { + case 1: // N + offset.mask = 0b11000001; + break; + case 2: // NE + offset.mask = 0b11100000; + break; + case 5: // E + offset.mask = 0b01110000; + break; + case 8: // SE + offset.mask = 0b00111000; + break; + case 7: // S + offset.mask = 0b00011100; + break; + case 6: // SW + offset.mask = 0b00001110; + break; + case 3: // W + offset.mask = 0b00000111; + break; + case 0: // NW + offset.mask = 0b10000011; + break; + case 4: // no offset -- fall through + default: // not reachable (no legal default) + offset.mask = 0; + break; + } + return offset; +} + + +// Supporting API +//////////////////////////////////////////////////////////////////////// + + +static struct drand48_data path_rand_; // If node at root+offset belongs as part of path, sets node values // (eg, weight and link to parent) @@ -26,13 +147,14 @@ // 'traversal_cost' is the cost to reach this node from the neighbor that // added it to the stack. void occupancy_grid_t::add_node_to_stack( - /* in */ const map_node_t& root_node, /* in */ const grid_index_t root_idx, /* in */ const node_offset_t offset, /* in */ const float traversal_cost ) { // Make sure we're not going off the edge of the map. If so, ignore. + assert(root_idx.idx >= 0); + map_node_t& root_node = m_grid[root_idx.idx]; int32_t new_x = (int32_t) root_node.pos.x + offset.dx; int32_t new_y = (int32_t) root_node.pos.y + offset.dy; if ((new_x < 0) || (new_x >= m_grid_size.cols) || (new_y < 0) || @@ -42,8 +164,8 @@ void occupancy_grid_t::add_node_to_stack( } ///////////////////////////////////////////////////////////////////// // Point is in the world. Check it. - uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); - map_node_t& child_node = grid->nodes[new_idx]; + grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; + map_node_t& child_node = m_grid[new_idx.idx]; if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { return; @@ -59,11 +181,11 @@ void occupancy_grid_t::add_node_to_stack( double jitter = 0.0; drand48_r(&path_rand_, &jitter); jitter = 0.1 * (jitter - 0.5); - float new_cost = root_node.path_cost + penalty + traversal_wt + jitter; - if (child_node->flags.state & PATH_NODE_FLAG_PROCESSED) { + float new_cost = root_node.path_cost + weight + traversal_cost + jitter; + if (child_node.flags.state & PATH_NODE_FLAG_PROCESSED) { // This node is already-processed. See if this path might provide // it a lower cost. - if (child_node->path_cost <= new_cost) + if (child_node.path_cost <= new_cost) { // Nope. return; @@ -72,10 +194,10 @@ void occupancy_grid_t::add_node_to_stack( // propagate updated weight to neighbors. } // Add point to stack for future consideration. - child_node->parent_id = root_idx; - child_node->weight = new_weight; - child_node->traversal_cost = penalty; - child_node->flags.state |= PATH_NODE_FLAG_PROCESSED; + child_node.parent_idx = root_idx; + child_node.path_cost = new_cost; + child_node.traversal_cost = weight; + child_node.flags.state |= PATH_NODE_FLAG_PROCESSED; m_queue.push(new_idx); } @@ -86,12 +208,13 @@ void occupancy_grid_t::add_node_to_stack( // to reach diagonal node // add pixel to stack for future neighbor analysis void occupancy_grid_t::add_node_to_stack_diag( - /* in */ const map_node_t& root_node, /* in */ const grid_index_t root_idx, /* in */ const node_offset_t offset ) { // make sure we're not going off the edge of the map + assert(root_idx.idx >= 0); + map_node_t& root_node = m_grid[root_idx.idx]; int32_t new_x = (int32_t) root_node.pos.x + offset.dx; int32_t new_y = (int32_t) root_node.pos.y + offset.dy; if ((new_x < 0) || (new_x >= m_grid_size.cols) @@ -101,8 +224,8 @@ void occupancy_grid_t::add_node_to_stack_diag( } ///////////////////////////////////////////////////////////////////// // point is in the world -- check it - uint32_t new_idx = (uint32_t) (new_x + new_y * m_grid_size.cols); - map_node_t& child_node = m_grid[new_idx]; + grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; + map_node_t& child_node = m_grid[new_idx.idx]; if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { return; @@ -130,28 +253,26 @@ void occupancy_grid_t::add_node_to_stack_diag( float cost = -1.0f; // get lowest traversal weight of vert and horiz paths and use that as // base for weight to diagonal node - if ((vert_child_node->flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { - cost = vert_child_node->traversal_cost; + if ((vert_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { + cost = vert_child_node.traversal_cost; } - if ((horiz_child_node->flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { - float h_cost = horiz_child_node->traversal_cost; + if ((horiz_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) == 0) { + float h_cost = horiz_child_node.traversal_cost; if (cost < 0.0f) { // Not accessible by vert node but horiz provides path. Use // horiz penalty. cost = h_cost; } else if ((h_cost > 0.0f) && (h_cost < cost)) { // Horiz node is passable and penalty is less than via vert, so - // use Horiz's penalty - cost = penalty; + // use horiz's penalty + cost = h_cost; } } assert(cost >= 0.0); // Cost needed to traverse to neighboring node. Scale it up because // distance is farther (it's on a diagonal). float traversal_weight = cost * 1.41f; - add_node_to_stack(root_node, root_idx, offset, traversal_weight); -end: - ; + add_node_to_stack(root_idx, offset, traversal_weight); } // pixel neighbors @@ -168,22 +289,21 @@ static const node_offset_t OFF_SW = { .dx=-1, .dy= 1 }; // pop the next node off the stack and processess it -static void occupancy_grod_t::process_next_stack_node() +void occupancy_grid_t::process_next_stack_node() { + grid_index_t root_idx = m_queue.front(); + m_queue.pop(); // Add 4-connected nodes to stack. - grid_index_t root_idx = m_queue.pop(); - map_node_t& root_node = m_grid[root_idx.idx]; - // - add_node_to_stack(root_node, root_idx, OFF_E); - add_node_to_stack(root_node, root_idx, OFF_W); - add_node_to_stack(root_node, root_idx, OFF_N); - add_node_to_stack(root_node, root_idx, OFF_S); + add_node_to_stack(root_idx, OFF_E); + add_node_to_stack(root_idx, OFF_W); + add_node_to_stack(root_idx, OFF_N); + add_node_to_stack(root_idx, OFF_S); // Add 8-connected nodes to stack. This will only happen if there's // a valid 4-connected path to get there. - add_node_to_stack_diag(root_node, root_idx, OFF_NE); - add_node_to_stack_diag(root_node, root_idx, OFF_NW); - add_node_to_stack_diag(root_node, root_idx, OFF_SE); - add_node_to_stack_diag(root_node, root_idx, OFF_SW); + add_node_to_stack_diag(root_idx, OFF_NE); + add_node_to_stack_diag(root_idx, OFF_NW); + add_node_to_stack_diag(root_idx, OFF_SE); + add_node_to_stack_diag(root_idx, OFF_SW); } @@ -200,7 +320,7 @@ void occupancy_grid_t::compute_path_costs() } // Now follow gradient in each node to build directional vector // to reach destination. - pixel_offset_bitfield_type base_direction, next_direction; + pixel_offset_bitfield_t base_direction, next_direction; // Iterate through nodes and build direction vector for each. Vector // is defined by following the path toward the destination (i.e., // stepping between adjacent nodes) out by several nodes, and measuing @@ -209,13 +329,13 @@ void occupancy_grid_t::compute_path_costs() for (uint32_t y=0; ynodes[idx]; + map_node_t& root = m_grid[idx]; map_node_t& ggp = root; // Find direction to ancestor. - for (uint32_t gen=0; gen= 0) + for (uint32_t gen=0; gennodes[ggp.parent_id.idx]; + map_node_t& next_ggp = m_grid[ggp.parent_idx.idx]; if (gen == 0) { // Get base direction. This returns bitfield @@ -245,10 +365,9 @@ void occupancy_grid_t::compute_path_costs() break; } } - path_offset_type dir; - float dx = (float) (ggp->pos.x - root->pos.x); - float dy = (float) (ggp->pos.y - root->pos.y); - float theta_degs = R2D * atan2f(dx, -dir.dy); + float dx = (float) (ggp.pos.x - root.pos.x); + float dy = (float) (ggp.pos.y - root.pos.y); + float theta_degs = c_rad_to_deg * atan2f(dx, -dy); if (theta_degs < 0.0) { theta_degs += 360.0; @@ -269,10 +388,10 @@ void occupancy_grid_t::add_anchor_to_path_stack( { assert(idx.idx < m_grid_size.cols * m_grid_size.rows); map_node_t& path_node = m_grid[idx.idx]; - path_node.weight = path_weight; - path_node.parent_id.val = -1; + path_node.path_cost = path_weight; + path_node.parent_idx.idx = c_invalid_grid_idx; m_queue.push(idx); - path_node->flags.state = PATH_NODE_FLAG_PROCESSED; + path_node.flags.state = PATH_NODE_FLAG_PROCESSED; } @@ -294,27 +413,35 @@ world_coordinate_t occupancy_grid_t::get_node_location( // If destination is outside of map bounds, only parent map is used // for path finding. void occupancy_grid_t::trace_routes(world_coordinate_t destination, - const occupancy_grid_t& parent_map) + occupancy_grid_t& parent_map) { clear(); // Add anchors. First destination, then boundary conditions. grid_index_t idx = get_node_index(destination.x_meters, - destination.y_meters; + destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); // For each boundary point, get distance/weight from parent map at // that location and use that value to set an anchor here. for (uint32_t y=0; y 0) && (y < m_grid_size.rows)) + if ((y > 0) && (y < m_grid_size.rows-1)) { // In this row, anchor first and last columns only - grid_coordinate_t pos = { .x=x, .y=y }; + grid_coordinate_t pos = { .x=0, .y=y }; world_coordinate_t world_pos = get_node_location(pos); // It's assumed that this map is a subset of parent map and // is fully contained in it as that's a design constraint. map_node_t& parent_node = parent_map.get_node(world_pos.x_meters, world_pos.y_meters); - add_anchor_to_path_stack(pos, parent_node.path_cost); + grid_index_t idx = { .idx = y * m_grid_size.cols }; + add_anchor_to_path_stack(idx, parent_node.path_cost); + // Last column. + pos.x = m_grid_size.rows - 1; + world_pos = get_node_location(pos); + parent_node = parent_map.get_node(world_pos.x_meters, + world_pos.y_meters); + idx.idx = y * m_grid_size.cols + (m_grid_size.cols - 1); + add_anchor_to_path_stack(idx, parent_node.path_cost); } else { @@ -327,7 +454,8 @@ void occupancy_grid_t::trace_routes(world_coordinate_t destination, // is fully contained in it as that's a design constraint. map_node_t& parent_node = parent_map.get_node( world_pos.x_meters, world_pos.y_meters); - add_anchor_to_path_stack(pos, parent_node.path_cost); + grid_index_t idx = { .idx = y * m_grid_size.cols + x }; + add_anchor_to_path_stack(idx, parent_node.path_cost); } } } @@ -335,12 +463,14 @@ void occupancy_grid_t::trace_routes(world_coordinate_t destination, } -void occupancy_grid_t::trace_routes(const world_coordinate_t& destination) +void occupancy_grid_t::trace_routes(world_coordinate_t destination) { clear(); grid_index_t idx = get_node_index(destination.x_meters, - destination.y_meters0; + destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); compute_path_costs(); } +} // namespace slam_sim + From 96a2c527a2e4d3a013724055b95d0635cb0b808c Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 6 Apr 2022 15:36:14 -0700 Subject: [PATCH 12/30] Snapshot save. Switching to other sub-repo --- slam_sim/src/Makefile | 2 +- slam_sim/src/main.cpp | 2 +- slam_sim/src/navigation.cpp | 43 +++---- slam_sim/src/support.cpp | 230 ++++++++++++++++++++++++++++++++++++ 4 files changed, 247 insertions(+), 30 deletions(-) create mode 100644 slam_sim/src/support.cpp diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 05d815c..bb324bc 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -10,7 +10,7 @@ endif TARGET = slam_sim -OBJS = main.o analyze.o occupancy.o rule_api.o path_map.o +OBJS = main.o analyze.o occupancy.o rule_api.o path_map.o support.o EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index dd221e6..612d36f 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -156,7 +156,7 @@ void init_sim() // Seed database and then create first path. // Seeding function manages its own transaction. gaia_log::app().info("Seeding the database..."); - seed_database(g_initial_x_meters, g_initial_y_meters); + slam_sim::seed_database(g_initial_x_meters, g_initial_y_meters); } } // namespace slam_sim; diff --git a/slam_sim/src/navigation.cpp b/slam_sim/src/navigation.cpp index 4522ad9..1d4b7cb 100644 --- a/slam_sim/src/navigation.cpp +++ b/slam_sim/src/navigation.cpp @@ -42,39 +42,26 @@ using gaia::slam::working_map_writer; using gaia::slam::error_correction_writer; -void calc_path_error(paths_t& path) +// Mock function to calculate error and optimize graph. +void optimize_graph(graphs_t& graph) { -gaia_log::app().info("Calculating error"); - observations_t head = path.first_observation(); - edges_t e = head.forward_edge(); - observations_t next = e.next(); - // TODO error can only be calculated when the start and end - // landmark is the same. - while (next) + gaia_log::app().info("Calculating error (mock)"); + for (observations_t o: graph.observations()) { - // TODO do something to estimate error - // For now, just iterate through the observations in the path. - gaia_log::app().info("Error obs {} at {},{}", next.id(), - next.pos_x_meters(), next.pos_y_meters()); - - if (!next.forward_edge()) - { - break; - } - next = next.forward_edge().next(); + // Just iterate through the observations in the graph to show + // how it's done. + gaia_log::app().info("Observaation {} at {},{}", o.id(), + o.position().pos_x_meters(), o.position().pos_y_meters()); } - - // Updated error correction table. - for (error_correction_t& ec: error_correction_t::list()) + for (edges_t e: graph.edges()) { - error_correction_writer writer = ec.writer(); - writer.correction_weight = ec.correction_weight() + 1.0; - writer.update_row(); - - // This isn't necessary as there's only one record, but it does - // help keep the code more clear. - break; + // Just iterate through the edges in the graph to show + // how it's done. + gaia_log::app().info("Edge connecting observations {} and {}", + e.src().id(), e.dest().id()); } + // When the graph is optimized, the position data for each observation + // is expected to be updated. } diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp new file mode 100644 index 0000000..45aa927 --- /dev/null +++ b/slam_sim/src/support.cpp @@ -0,0 +1,230 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////// +// +// Supporting functions. +// +//////////////////////////////////////////////////////////////////////// + +#include +#include + +#include + +#include +#include + +#include "constants.hpp" +#include "slam_sim.hpp" +//#include "line_segment.hpp" + + +namespace slam_sim +{ + +//static int32_t s_next_observation_id = 1; +// +using gaia::common::gaia_id_t; +// +using gaia::slam::area_map_t; +//using gaia::slam::edges_t; +using gaia::slam::destination_t; +using gaia::slam::ego_t; +using gaia::slam::graphs_t; +using gaia::slam::latest_observation_t; +//using gaia::slam::movements_t; +//using gaia::slam::observations_t; +using gaia::slam::observed_area_t; +using gaia::slam::observed_area_t; +using gaia::slam::working_map_t; +//using gaia::slam::positions_t; +//using gaia::slam::range_data_t; +// +//using gaia::slam::latest_observation_writer; +//using gaia::slam::observed_area_writer; +// +// +////////////////////////////////////////////////////////////////////////// +//// Background API that supports the contents of rule_api.cpp. Most +//// importantly, this means that the functions here are expected to +//// be called from within an active transaction +// +//static void update_observed_area(ego_t& ego, map_coord_t coord) +//{ +// observed_area_t area = ego.world(); +// bool change = false; +// // Left and right bounds. +// float left_edge = area.left_meters(); +// float right_edge = area.right_meters(); +// if (floor(coord.x_meters - (c_range_sensor_max_meters+1)) < left_edge) +// { +// left_edge = floor(coord.x_meters - (c_range_sensor_max_meters+1)); +// change = true; +// } +// if (ceil(coord.x_meters + (c_range_sensor_max_meters+1)) > right_edge) +// { +// right_edge = floor(coord.x_meters + (c_range_sensor_max_meters+1)); +// change = true; +// } +// // Top and bottom bounds. +// float bottom_edge = area.bottom_meters(); +// float top_edge = area.top_meters(); +// if (floor(coord.y_meters - (c_range_sensor_max_meters+1)) < bottom_edge) +// { +// bottom_edge = floor(coord.y_meters - (c_range_sensor_max_meters+1)); +// change = true; +// } +// if (ceil(coord.y_meters + (c_range_sensor_max_meters+1)) > top_edge) +// { +// top_edge = floor(coord.y_meters + (c_range_sensor_max_meters+1)); +// change = true; +// } +// // If bounds were modified, update observed area record. +// if (change) +// { +// observed_area_writer oa_writer = area.writer(); +// oa_writer.left_meters = left_edge; +// oa_writer.right_meters = right_edge; +// oa_writer.bottom_meters = bottom_edge; +// oa_writer.top_meters = top_edge; +// oa_writer.update_row(); +// } +//} +// +//void create_observation(map_coord_t& prev, map_coord_t& coord) +//{ +// uint32_t obs_num = s_next_observation_id++; +// gaia_log::app().info("Performing observation {} at {},{} heading {}", +// obs_num, coord.x_meters, coord.y_meters, coord.heading_degs); +// sensor_data_t data; +// calculate_range_data(coord, data); +// +// // Get ego +// ego_t& ego = *(ego_t::list().begin()); +// uint32_t graph_id = ego.current_graph_id(); +// +// // create position record +// gaia_id_t pos_id = positions_t::insert_row( +// coord.x_meters, // x_meters +// coord.y_meters, // y_meters +// coord.heading_degs // heading_degs +// ); +// // create range_data record +// gaia_id_t range_id = range_data_t::insert_row( +// data.num_radials, // num_radials +// data.bearing_degs, // bearing_degs +// data.range_meters // distance_meters +// ); +// // create movement record +// gaia_id_t movement_id = movements_t::insert_row( +// coord.x_meters - prev.x_meters, // dx_meters +// coord.y_meters - prev.y_meters, // dy_meters +// coord.heading_degs - prev.heading_degs // dheading_degs +// ); +// // create observation record +// gaia_id_t observation_id = observations_t::insert_row( +// obs_num, // id +// graph_id // graph_id +// ); +// +// // create references +// observations_t obs = observations_t::get(observation_id); +// obs.position().connect(pos_id); +// obs.range_data().connect(range_id); +// obs.motion().connect(movement_id); +// +// update_observed_area(ego, coord); +// +// // create edge +// latest_observation_t lo = ego.latest_observation(); +// observations_t prev_obs = lo.observation(); +// if (prev_obs) +// { +// edges_t::insert_row( +// graph_id, // graph_id +// prev_obs.id(), // src_id +// obs_num // dest_id +// ); +// } +// +// // update latest_observation +// latest_observation_writer lo_writer = lo.writer(); +// lo_writer.observation_id = obs_num; +// lo_writer.update_row(); +//} +// + +//////////////////////////////////////////////////////////////////////// +// Non-rule API +// The functions here must manage their own transactions. + +void seed_database(float x_meters, float y_meters) +{ + // There shouldn't be any transaction conflicts as this is the first + // operation on the database so ignore the try/catch block. + gaia::db::begin_transaction(); + + //////////////////////////////////////////// + // Records + graphs_t::insert_row(1); // id + gaia_id_t ego_id = ego_t::insert_row(1); // current_graph_id + ego_t ego = ego_t::get(ego_id); + + gaia_id_t latest_observation_id = latest_observation_t::insert_row( + 0 // observation_id + ); + + gaia_id_t world_id = observed_area_t::insert_row( + floor(x_meters - (c_range_sensor_max_meters+1)), // left_meters + ceil(x_meters + (c_range_sensor_max_meters+1)), // right_meters + ceil(y_meters + (c_range_sensor_max_meters+1)), // top_meters + floor(y_meters - (c_range_sensor_max_meters+1)) // bottom_meters + ); + + gaia_id_t area_id = area_map_t::insert_row( + 0, // blob_id + floor(x_meters - (c_range_sensor_max_meters+1)), // left_meters + ceil(x_meters + (c_range_sensor_max_meters+1)), // right_meters + ceil(y_meters + (c_range_sensor_max_meters+1)), // top_meters + floor(y_meters - (c_range_sensor_max_meters+1)), // bottom_meters + 0, // num_rows + 0 // num_cols + ); + + gaia_id_t working_id = working_map_t::insert_row( + 0, // blob_id + floor(x_meters - (c_range_sensor_max_meters)), // left_meters + ceil(x_meters + (c_range_sensor_max_meters)), // right_meters + ceil(y_meters + (c_range_sensor_max_meters)), // top_meters + floor(y_meters - (c_range_sensor_max_meters)), // bottom_meters + 0, // num_rows + 0 // num_cols + ); + + gaia_id_t destination_id = destination_t::insert_row( + x_meters, // x_meters + y_meters, // y_meters + 0.0 // departure_time_sec + ); + + //////////////////////////////////////////// + // Relationships + ego.destination().connect(destination_id); + ego.world().connect(world_id); + ego.latest_observation().connect(latest_observation_id); + ego.low_res_map().connect(area_id); + ego.working_map().connect(working_id); + + //////////////////////////////////////////// + // All done + gaia::db::commit_transaction(); +} + +} // namespace slam_sim + From 3e2539dff288f443a8a4d3d92ff342f398665334 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Tue, 12 Apr 2022 20:44:59 -0700 Subject: [PATCH 13/30] Snapshot save. Integrating blobs w/ maps. --- slam_sim/gaia/slam.ddl | 52 ++++---- slam_sim/gaia/slam.ruleset | 62 +++++---- slam_sim/include/blob_cache.hpp | 23 ++-- slam_sim/include/occupancy.hpp | 66 ++++++++-- slam_sim/include/slam_sim.hpp | 17 ++- slam_sim/src/main.cpp | 2 +- slam_sim/src/navigation.cpp | 90 ++++++------- slam_sim/src/occupancy.cpp | 209 +++++++++++++++++++++--------- slam_sim/src/rule_api.cpp | 32 +++-- slam_sim/src/support.cpp | 54 +++++--- slam_sim/src/utils/blob_cache.cpp | 25 ++-- 11 files changed, 387 insertions(+), 245 deletions(-) diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 6a7740c..efc2d6d 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -13,7 +13,7 @@ -- Schema is split into 3 categories of tables. -- -- The first includes tables that have individual records in them. For --- example, the 'ego', the 'position', and maps that represent the +-- example, the 'ego', the 'destination', and maps that represent the -- area (used for path finding). -- -- The second includes tables storing event data. This includes destination @@ -21,8 +21,7 @@ -- to go to a particular position) or if Alice senses a collision. -- -- The third includes the main elements of the SLAM algorithm. This includes --- observations (more commonly called poses) and sequences of --- observations here called paths (akin to simplified pose graphs). +-- graphs, edges, and vertices, and auxiliary tables to support these. -- ------------------------------------------------------------------------ @@ -42,13 +41,17 @@ database slam; -- -- Data tables: -- graphs Group of observations to produce pose graph. --- observations Synonymous with node, pose, vertex. +-- vertices Where an observation is made (aka: pose, node). -- positions Position associated with observation. -- movements Movement to reach this observation. -- range_data Sensor range data for an observation. -- edges Connects two observations. -- error_corrections Information from graph optimization round. +-- Note that 'vertex' and 'observation' are often used interchangeaby. +-- In future, these should be split. An 'observation' is a view of the +-- environment, and that may or may not become a vertex, while a vertex +-- will always be associated with an observation. ------------------------------------------------------------------------ -- Tables with single records (i.e., exactly one) @@ -63,7 +66,7 @@ table ego current_graph references graphs where ego.current_graph_id = graphs.id, - -- Most recent timestmap is stored in most recent observation + -- Most recent timestmap is stored in most recent observation/vertex -- (referenced below). -- Explicitly created references. @@ -134,10 +137,12 @@ table working_map -- Bounding polygon -- Uses world coordinates, with increasing X,Y being rightward/upward - left_meters float, - right_meters float, - top_meters float, - bottom_meters float, + width_meters float, + height_meters float, +-- left_meters float, +-- right_meters float, +-- top_meters float, +-- bottom_meters float, -- Grid size num_rows uint32, num_cols uint32, @@ -171,9 +176,9 @@ table destination -- Represents the most recently created observation. table latest_observation ( - observation_id uint32, - observation references observations - where latest_observation.observation_id = observations.id, + vertex_id uint32, + vertex references vertices + where latest_observation.vertex_id = vertices.id, ego references ego ) @@ -207,7 +212,7 @@ table graphs ( id uint32 unique, - observations references observations[], + vertices references vertices[], edges references edges[], -- Reverse reference to ego (necessary until one-way references are @@ -217,8 +222,7 @@ table graphs ------------------------------------------------ --- Observation related --- TODO rename 'observations' to either 'nodes', 'poses', or 'vertices'. +-- Vertex related -- An observation from a point in the world, including sensor readings -- from this point and a link to other nearby observations. This is @@ -226,7 +230,7 @@ table graphs -- Most of observation data in kept different tables so that updates to -- one field don't risk conflict and txn rollback if others are -- modified. -table observations +table vertices ( ------------------------------ -- Constant data. These values are set at creation and don't change. @@ -241,7 +245,7 @@ table observations graph_id uint32, graph references graphs - where graphs.id = observations.graph_id, + where graphs.id = vertices.graph_id, timestamp_sec double, @@ -269,7 +273,7 @@ table positions heading_degs float, -- Constant reference established at record creation time. - observation references observations + vertex references vertices ) @@ -283,7 +287,7 @@ table movements dheading_degs float, -- Constant reference established at record creation time. - observation references observations + vertex references vertices ) @@ -295,7 +299,7 @@ table range_data distance_meters float[], -- Constant reference established at record creation time. - observation references observations + vertex references vertices ) @@ -315,12 +319,12 @@ table edges -- they won't be (e.g., if an edge is formed between observation Y -- and existing observation M). src_id uint32, - src references observations - using out_edges where edges.src_id = observations.id, + src references vertices + using out_edges where edges.src_id = vertices.id, dest_id uint32, - dest references observations - using in_edges where edges.dest_id = observations.id + dest references vertices + using in_edges where edges.dest_id = vertices.id ) diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index f9db2a8..e3d2a78 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -22,35 +22,33 @@ #include "slam_sim.hpp" - -using slam_sim::optimization_required; -using slam_sim::optimize_graph; - -using slam_sim::need_to_extend_map; -using slam_sim::build_area_map; -using slam_sim::build_working_map; - -using slam_sim::select_new_destination; -using slam_sim::move_toward_destination; -using slam_sim::full_stop; - - /*********************************************************************** Rules map_ruleset serial_group on_insert(edges) If optimization required, do so and rebuild maps. on_change(destination) Rebuild working map, move to destination. - on_insert(observation) Extend area map if necessary. + on_insert(vertices) Extend area map if necessary. ruleset - on_insert(observation) Spatial query to link to other nodes. - on_insert(observation) Determine if it's time to change destination. - on_insert(collision_event) Full stop and create observation. + on_insert(vertices) Spatial query to link to other nodes. + on_insert(vertices) Determine if it's time to change destination. + on_insert(collision_event) Full stop and create vertex. ***********************************************************************/ +using slam_sim::optimization_required; +using slam_sim::optimize_graph; + +using slam_sim::need_to_extend_map; +using slam_sim::build_area_map; +using slam_sim::build_working_map; + +using slam_sim::select_new_destination; +using slam_sim::move_toward_destination; +using slam_sim::full_stop; + // Wrap error correction and map creation in a serial group as these are // expensive operations and we want to avoid concurrent operations @@ -58,19 +56,19 @@ ruleset ruleset map_ruleset : serial_group() { // We don't want multiple edges to be created at similar points in time - // and both realize that it's time to optimize the map, and do so - // together, so keep the graph optimization check in a serial group. + // and both edges realize that it's time to optimize the map, so keep + // the graph optimization check in a serial group. on_insert(edges) { // New edge created. See if it's time to run new graph optimization. if (optimization_required()) { - optimize_graph(edges.dest->observations.graph->graphs); + optimize_graph(edges.dest->vertices.graph->graphs); // Whenever new error-correction data is available, regenerate // maps. This can be triggered in a separate rule or simply // done here, so do it here. - build_area_map(/area_map, /observed_area); + build_area_map(/destination, /area_map, /observed_area); build_working_map(/destination, /area_map, /working_map); } } @@ -84,8 +82,8 @@ ruleset map_ruleset : serial_group() } - // For each observation, update working map using new sensor data. - on_insert(observations) + // For each vertex, update working map using new sensor data. + on_insert(vertices) { // If working map will go beyond border of area map, rebuild // area map. @@ -100,32 +98,32 @@ ruleset map_ruleset : serial_group() ruleset runtime_ruleset { - // Observations are created at a lower infrastructure level. The + // Vertices are created at a lower infrastructure level. The // decision to do so is based on the following assumptions about // when an observation is made: - // 1) Observations (nodes) are generated at locations that provide + // 1) Vertices are generated at locations that provide // non-redundant information about the environment, such as range // data that covers previously unexplored territory or being // near salient landmark to form closures with other nearby - // observations. - // 2) Sensor data is read at a rate higher than observations are + // vertices. + // 2) Sensor data is read at a rate higher than vertices are // created. // 3) The time required to evaluate whether or not to make an // observation may be longer than the interval between successive // packets of sensor data. // This latter assumption means that one can't directly use a rule - // triggering of sensor data to build observations as these must - // run in parallel, risking violation of (1), as other observations + // triggering of sensor data to build vertices as these must + // run in parallel, risking violation of (1), as other vertices // made during processing will be invisible to the rule's txn. - on_insert(observations) + on_insert(vertices) { // TODO Perform spatial query in area of observation and see if - // there are other observations to build edges with. + // there are other vertices to build edges with. } - on_insert(observations) + on_insert(vertices) { // Determine if it's time to change destinations. select_new_destination(); diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp index 92d1fd5..0abff9c 100644 --- a/slam_sim/include/blob_cache.hpp +++ b/slam_sim/include/blob_cache.hpp @@ -28,6 +28,9 @@ #include #include +namespace slam_sim +{ + constexpr uint32_t c_invalid_blob_id = 0; enum class blob_state_t : uint8_t @@ -68,27 +71,23 @@ struct blob_t class blob_cache_t { public: - static blob_cache_t* get(); + blob_cache_t() = default; + blob_cache_t(const blob_cache_t&) = delete; + blob_cache_t& operator=(const blob_cache_t&) = delete; -public: // Creates a blob with the specified ID and characteristics - blob_t* create_or_reset_blob(uint32_t id, size_t size, uint32_t id_superseded_blob = c_invalid_blob_id); + blob_t* create_or_reset_blob(uint32_t id, size_t size, + uint32_t id_superseded_blob = c_invalid_blob_id); // Retrieves a blob given an id. // Will return nullptr if the blob doesn't exist. blob_t* get_blob(uint32_t id); -protected: - blob_cache_t() = default; - - blob_cache_t(const blob_cache_t&) = delete; - blob_cache_t& operator=(const blob_cache_t&) = delete; - protected: // Map of allocated blobs. std::unordered_map m_blob_map; std::shared_mutex m_lock; - -protected: - static blob_cache_t s_blob_cache; }; + +} // namespace slam_sim + diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 387d2a3..9f9549e 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -147,10 +147,10 @@ struct map_node_t class occupancy_grid_t { public: - occupancy_grid_t(float node_width_meters, world_coordinate_t top_left, - world_coordinate_t bottom_right); - occupancy_grid_t(gaia::slam::area_map_t&); - occupancy_grid_t(gaia::slam::working_map_t&); + // Creates new uncached map. + occupancy_grid_t(float node_width_meters, world_coordinate_t bottom_left, + float width_meters, float height_meters); + ~occupancy_grid_t(); // Resets the map grid. @@ -160,19 +160,43 @@ class occupancy_grid_t map_node_t& get_node(float x_meters, float y_meters); map_node_flags_t& get_node_flags(float x_meters, float y_meters); - // Apply sensor data to map from observation. - void apply_sensor_data(const gaia::slam::observations_t&); + // Apply sensor data to map from vertex. + void apply_sensor_data(const gaia::slam::vertices_t&); void export_as_pnm(std::string file_name); // Path finding. Traces all routes to the destination. If map is // embedded in larger map, the larger map is used to set boundary // conditions for this map. - void trace_routes(world_coordinate_t destination, + void trace_routes(world_coordinate_t& destination, occupancy_grid_t& parent_map); - void trace_routes(world_coordinate_t destination); + void trace_routes(world_coordinate_t& destination); + + // Provide interface to blob ID in use so that can be pushed + // to the database. + int32_t get_blob_id() { return m_blob_id; } + + grid_size_t get_grid_size() { return m_grid_size; } + world_coordinate_t get_bottom_left() { return m_bottom_left; } + map_size_t get_map_size() { return m_map_size; } protected: + // Default constructor is used by subclasses. They're expected to + // do their own construction that's consistent and compatible with + // superclass. They really aren't subclasses in a traditional + // sense, as they don't extend the functionality of the base class, + // but they do represent different types, one each for different + // singleton map database tables, and this class is cleaner + // when they're moved onto their own. + occupancy_grid_t() { } + // Contructor helper function, with code common between constructors. + void init(float node_width_meters, world_coordinate_t bottom_left, + float width_meters, float height_meters); + // Allocates own memory for m_grid, which is freed on destruction. + // Grids created from DB blobs have memory for m_grid that is + // managed externally. + void allocate_own_grid(); + grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); void apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters); @@ -229,5 +253,31 @@ class occupancy_grid_t const float path_weight); }; + +class area_grid_t : public occupancy_grid_t +{ +public: + // Constructors to be called by rules. + // Loads existing map, if present (map will be blank if it's doesn't + // exist yet). + area_grid_t(gaia::slam::area_map_t&); + // Purges existing map and rebuilds a new one, using observed area as + // bounds. + area_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); +}; + + +class working_grid_t : public occupancy_grid_t +{ +public: + // Creates a grid that's not associated w/ database record. + working_grid_t(); + + // Constructor to be called by rules. + // Loads existinig working map. If this is to be embedded in larger + // map, that must be done separately. + working_grid_t(gaia::slam::working_map_t&); +}; + } // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 8257985..951f986 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -23,6 +23,8 @@ #pragma once #include "gaia_slam.h" + +#include "blob_cache.hpp" #include "occupancy.hpp" #include "sensor_data.hpp" @@ -33,6 +35,9 @@ namespace slam_sim // to quit it's set to 1. extern int32_t g_quit; +// Blob caches for area maps and working maps. +extern blob_cache_t g_area_blobs; +extern blob_cache_t g_working_blobs; //////////////////////////////////////////////// // Rules API @@ -44,7 +49,7 @@ extern int32_t g_quit; bool optimization_required(); // Stub function to graph optimization. Contents of function show how -// to iterate through a graph's nodes ('observations') and edges. +// to iterate through a graph's vertices and edges. void optimize_graph(gaia::slam::graphs_t&); @@ -80,14 +85,14 @@ void full_stop(); //////////////////////////////////////////////// // Support API -// Creates a record in the observation table using the supplied sensor +// Creates a record in the vertices table using the supplied sensor // data -void create_observation(const slam_sim::sensor_data_t& data); +void create_vertex(const slam_sim::sensor_data_t& data); // Do a sensor sweep from at the stated position. This would normally be // pushed up from the sensor/hardware layer, but polling for it makes // the simulation more straightforward. -void generate_sensor_data(double pos_x_meters, double pos_y_meters, +void generate_sensor_data(float pos_x_meters, float pos_y_meters, sensor_data_t& data); // Generates and exports a map to disk. Filename will be based on ego's @@ -95,14 +100,14 @@ void generate_sensor_data(double pos_x_meters, double pos_y_meters, void export_map_to_file(); // External request to move to a specific location. -void request_destination(double x_meters, double y_meters); +void request_destination(float x_meters, float y_meters); //////////////////////////////////////////////// // Initialization // Params are Alice's starting point. -void seed_database(double initial_x_meters, double iniital_y_meters); +void seed_database(float initial_x_meters, float iniital_y_meters); // Loads a .json file that describes the environment. void load_world_map(const char*); diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 612d36f..7cc728f 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -75,7 +75,7 @@ void clean_db() remove_all_rows(); remove_all_rows(); remove_all_rows(); - remove_all_rows(); + remove_all_rows(); remove_all_rows(); remove_all_rows(); remove_all_rows(); diff --git a/slam_sim/src/navigation.cpp b/slam_sim/src/navigation.cpp index 1d4b7cb..8ac2c24 100644 --- a/slam_sim/src/navigation.cpp +++ b/slam_sim/src/navigation.cpp @@ -28,101 +28,85 @@ namespace slam_sim using std::string; -using gaia::slam::paths_t; -using gaia::slam::observations_t; +using gaia::slam::graphs_t; using gaia::slam::edges_t; +using gaia::slam::vertices_t; + using gaia::slam::area_map_t; -using gaia::slam::local_map_t; using gaia::slam::working_map_t; -using gaia::slam::error_correction_t; +//using gaia::slam::error_corrections_t; using gaia::slam::area_map_writer; -using gaia::slam::local_map_writer; -using gaia::slam::working_map_writer; -using gaia::slam::error_correction_writer; +//using gaia::slam::working_map_writer; // Mock function to calculate error and optimize graph. void optimize_graph(graphs_t& graph) { gaia_log::app().info("Calculating error (mock)"); - for (observations_t o: graph.observations()) + for (vertices_t v: graph.observations()) { // Just iterate through the observations in the graph to show // how it's done. - gaia_log::app().info("Observaation {} at {},{}", o.id(), - o.position().pos_x_meters(), o.position().pos_y_meters()); + gaia_log::app().info("Vertex {} at {},{}", v.id(), + v.position().pos_x_meters(), v.position().pos_y_meters()); } for (edges_t e: graph.edges()) { // Just iterate through the edges in the graph to show // how it's done. - gaia_log::app().info("Edge connecting observations {} and {}", + gaia_log::app().info("Edge connecting vertices {} and {}", e.src().id(), e.dest().id()); } // When the graph is optimized, the position data for each observation - // is expected to be updated. + // is subject to being updated. } -void export_area_map() -{ - static int32_t ctr = 0; - string fname("map_" + std::to_string(ctr) + ".pgm"); - gaia_log::app().info("Building map {}", fname); - g_area_map->export_as_pnm(fname); - ctr++; -} +//void export_area_map() +//{ +// static int32_t ctr = 0; +// string fname("map_" + std::to_string(ctr) + ".pgm"); +// gaia_log::app().info("Building map {}", fname); +// g_area_map->export_as_pnm(fname); +// ctr++; +//} -void build_area_map(area_map_t& am) +// TODO move to rules_api.cpp +void build_area_map(destination_t& dest, area_map_t& am, + observed_area_t& bounds) { - g_area_map->clear(); - for (paths_t& p: paths_t::list()) +#warning "Need to adapt constructor to accept new bounds" + // Each time we build an area map + occupancy_grid_t area_map(am, true); + for (graphs_t& g: graphs_t::list()) { - observations_t obs = p.first_observation(); - while (obs) + for (vertex_t& v: g.vertices()) { - // TODO do something with observation data. gaia_log::app().info("Pulling sensor data from {}:{}", - p.id(), obs.id()); - g_area_map->apply_sensor_data(obs); - if (!obs.forward_edge()) { - break; - } - obs = obs.forward_edge().next(); + g.id(), v.id()); + area_map->apply_sensor_data(v); } } - - // TODO rebuild the area map - // In the meantime, just 'touch' the record by updating the - // change counter. + // Build paths to destination + area_map.trace_routes(dest); + // Update blob ID in database area_map_writer writer = am.writer(); - writer.change_counter = am.change_counter() + 1; + writer.blob_id = area_map.blob_id(); writer.update_row(); - - export_area_map(); +// export_area_map(); } -void build_local_map(local_map_t& lm) -{ - // TODO rebuild the local map - // In the meantime, just 'touch' the record by updating the - // change counter. - local_map_writer writer = lm.writer(); - writer.change_counter = lm.change_counter() + 1; - writer.update_row(); -} - -void build_working_map(working_map_t& wm) +void build_working_map(destination_t& dest, working_map_t& wm) { // TODO rebuild the working map // In the meantime, just 'touch' the record by updating the // change counter. - working_map_writer writer = wm.writer(); - writer.change_counter = wm.change_counter() + 1; - writer.update_row(); +// working_map_writer writer = wm.writer(); +// writer.change_counter = wm.change_counter() + 1; +// writer.update_row(); } } // namespace slam_sim diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 5532a1f..eec945c 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include "blob_cache.hpp" #include "constants.hpp" #include "line_segment.hpp" @@ -31,22 +33,26 @@ using std::vector; using std::string; using std::cerr; -using gaia::slam::observations_t; +using gaia::slam::vertices_t; using gaia::slam::positions_t; using gaia::slam::range_data_t; using gaia::slam::area_map_t;; using gaia::slam::working_map_t;; +using gaia::slam::observed_area_t;; + +using gaia::slam::area_map_writer; + +blob_cache_t g_area_blobs; +blob_cache_t g_working_blobs; //////////////////////////////////////////////////////////////////////// -// Constructors and destructors +// Construction and destruction -occupancy_grid_t::occupancy_grid_t(float node_width_meters, - world_coordinate_t top_left, world_coordinate_t bottom_right) +void occupancy_grid_t::init(float node_width_meters, + world_coordinate_t bottom_left, float width_meters, float height_meters) { - float width_meters = bottom_right.x_meters - top_left.x_meters; - float height_meters = top_left.y_meters - bottom_right.y_meters; // Number of grids in map. m_grid_size.rows = (uint32_t) ceil(height_meters / node_width_meters); m_grid_size.cols = (uint32_t) ceil(width_meters / node_width_meters); @@ -55,64 +61,26 @@ occupancy_grid_t::occupancy_grid_t(float node_width_meters, // Physical size of map. m_map_size.x_meters = width_meters; m_map_size.y_meters = height_meters; - // Map center. - m_bottom_left.x_meters = top_left.x_meters; - m_bottom_left.y_meters = bottom_right.y_meters; - // - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - // Signal that this owns the memory allocation. - m_blob_id = -1; - m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); - clear(); + // Map placement. + m_bottom_left = bottom_left; } -occupancy_grid_t::occupancy_grid_t(area_map_t& am) +void occupancy_grid_t::allocate_own_grid() { - m_node_size_meters = c_area_map_node_width_meters; - // Get map size and bounds. - m_grid_size.rows = am.num_rows(); - m_grid_size.cols = am.num_cols(); - m_map_size.x_meters = am.right_meters() - am.left_meters(); - m_map_size.y_meters = am.top_meters() - am.bottom_meters(); - m_bottom_left.x_meters = am.left_meters(); - m_bottom_left.y_meters = am.bottom_meters(); - // Recover memory from blob cache. + // Signal that 'this' owns the memory allocation. + m_blob_id = -1; uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_blob_id = am.blob_id(); - blob_t* blob = blob_cache_t::get()->get_blob(m_blob_id); - if (blob == NULL) - { - // Blob doesn't exist yet. Create it. - size_t sz = num_nodes * sizeof *m_grid; - blob = blob_cache_t::get()->create_or_reset_blob(am.blob_id(), sz); - } - m_grid = (map_node_t*) blob->data; - clear(); + m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); } -occupancy_grid_t::occupancy_grid_t(working_map_t& wm) +// Creates new uncached map. +occupancy_grid_t::occupancy_grid_t(float node_width_meters, + world_coordinate_t bottom_left, float width_meters, float height_meters) { - m_node_size_meters = c_working_map_node_width_meters; - // Get map size and bounds. - m_grid_size.rows = wm.num_rows(); - m_grid_size.cols = wm.num_cols(); - m_map_size.x_meters = wm.right_meters() - wm.left_meters(); - m_map_size.y_meters = wm.top_meters() - wm.bottom_meters(); - m_bottom_left.x_meters = wm.left_meters(); - m_bottom_left.y_meters = wm.bottom_meters(); - // Recover memory from blob cache. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_blob_id = wm.blob_id(); - blob_t* blob = blob_cache_t::get()->get_blob(m_blob_id); - if (blob == NULL) - { - // Blob doesn't exist yet. Create it. - size_t sz = num_nodes * sizeof *m_grid; - blob = blob_cache_t::get()->create_or_reset_blob(wm.blob_id(), sz); - } - m_grid = (map_node_t*) blob->data; + init(node_width_meters, bottom_left, width_meters, height_meters); + allocate_own_grid(); clear(); } @@ -287,7 +255,7 @@ void occupancy_grid_t::apply_flags() } -void occupancy_grid_t::apply_sensor_data(const observations_t& obs) +void occupancy_grid_t::apply_sensor_data(const vertices_t& obs) { positions_t pos = obs.position(); range_data_t r = obs.range_data(); @@ -311,9 +279,9 @@ void occupancy_grid_t::apply_sensor_data(const observations_t& obs) void occupancy_grid_t::export_as_pnm(string file_name) { uint32_t n_pix = m_grid_size.cols * m_grid_size.rows; - uint8_t* r = new uint8_t[n_pix]; - uint8_t* g = new uint8_t[n_pix]; - uint8_t* b = new uint8_t[n_pix]; + uint8_t* r = (uint8_t*) calloc(1, n_pix); + uint8_t* g = (uint8_t*) calloc(1, n_pix); + uint8_t* b = (uint8_t*) calloc(1, n_pix); // Create image. for (uint32_t y=0; ydata; +} + + +// Overwrites existing cached area map. +area_grid_t::area_grid_t(area_map_t& am, observed_area_t& area) +{ + world_coordinate_t bottom_left = { + .x_meters = am.left_meters(), + .y_meters = am.bottom_meters() + }; + init(c_area_map_node_width_meters, bottom_left, + area.right_meters() - area.left_meters(), + area.top_meters() - area.bottom_meters()); + // Get rid of old content and create a new empty blob. + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + uint32_t old_blob_id = am.blob_id(); + size_t sz = num_nodes * sizeof *m_grid; + m_blob_id = old_blob_id + 1; + blob_t* blob = g_area_blobs.create_or_reset_blob(m_blob_id, sz, + old_blob_id); + m_grid = (map_node_t*) blob->data; + clear(); + // Store changes in DB. + area_map_writer writer = am.writer(); + writer.blob_id = m_blob_id; + writer.left_meters = area.left_meters(); + writer.right_meters = area.right_meters(); + writer.top_meters = area.top_meters(); + writer.bottom_meters = area.bottom_meters(); + writer.update_row(); +} + +//////////////////////////////////////////////////////////////////////// +// working grid subclass + +// Load existing working map. +working_grid_t::working_grid_t() +{ + + world_coordinate_t bottom_left = { + .x_meters = floorf(-c_range_sensor_max_meters), + .y_meters = floorf(-c_range_sensor_max_meters) + }; + float right_meters = ceilf(c_range_sensor_max_meters); + float top_meters = ceilf(c_range_sensor_max_meters); + init(c_working_map_node_width_meters, bottom_left, + right_meters - bottom_left.x_meters, + top_meters - bottom_left.y_meters); + allocate_own_grid(); +} + +// Load existing working map. +working_grid_t::working_grid_t(working_map_t& wm) +{ + world_coordinate_t bottom_left = { + .x_meters = wm.left_meters(), + .y_meters = wm.bottom_meters() + }; + init(c_working_map_node_width_meters, bottom_left, + wm.right_meters() - wm.left_meters(), + wm.top_meters() - wm.bottom_meters()); + assert(m_grid_size.rows = wm.num_rows()); + assert(m_grid_size.cols = wm.num_cols()); + // Recover memory from blob cache. + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + m_blob_id = wm.blob_id(); + assert(m_blob_id != c_invalid_blob_id); + blob_t* blob = g_working_blobs.get_blob(m_blob_id); + if (blob == NULL) + { + // Blob doesn't exist yet. This means that the process was + // started up against an existing database. This shouldn't + // happen, but allow it. Create it. + gaia_log::app().warn("Area map with ID {} did not have cached " + "blob. Recreating one.", m_blob_id); + size_t sz = num_nodes * sizeof *m_grid; + blob = g_working_blobs.create_or_reset_blob(wm.blob_id(), sz); + // New grid. Nodes need initialization. + clear(); + } + m_grid = (map_node_t*) blob->data; +} } // namespace slam_sim diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 7bd5ca7..34b6943 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -34,7 +34,7 @@ using gaia::slam::ego_t; using gaia::slam::error_corrections_t; using gaia::slam::graphs_t; using gaia::slam::observed_area_t; -using gaia::slam::observations_t; +using gaia::slam::vertices_t; using gaia::slam::area_map_t; using gaia::slam::working_map_t; @@ -43,7 +43,7 @@ using gaia::slam::working_map_t; // In a live example, this function would apply logic to determine if // enough data has been collected (e.g., new range data, new closures, // etc.) to justify performing another map optimization. For now, say -// an optiimzation is required every X observations. +// an optiimzation is required every X vertices. bool optimization_required() { static int32_t ctr = 0; @@ -69,11 +69,11 @@ void optimize_graph(graphs_t& graph) // By edges: for (edges_t& e: edges_t::list()) { - gaia_log::app().info("Edge connects observations {} and {}", + gaia_log::app().info("Edge connects vertices {} and {}", e.src().id(), e.dest().id()); } - // By observations: - for (observations_t& o: observations_t::list()) + // By vertices: + for (vertices_t& o: vertices_t::list()) { gaia_log::app().info("Obervation {} connects to:", o.id()); for (edges_t& e: o.in_edges()) @@ -108,16 +108,16 @@ bool need_to_extend_map() } -static void build_map(const graphs_t& g, const world_coordinate_t& top_left, - const world_coordinate_t bottom_right) +static void build_map(const graphs_t& g, const world_coordinate_t& bottom_left, + float width_meters, float height_meters) { static int32_t ctr = 0; - occupancy_grid_t map(c_standard_map_node_width_meters, - top_left, bottom_right); + occupancy_grid_t map(c_standard_map_node_width_meters, bottom_left, + width_meters, height_meters); // Iterate through observations in this graph and build a map. - for (const observations_t& o: g.observations()) + for (const vertices_t& o: g.vertices()) { - gaia_log::app().info("Applying sensor data from observation {}", + gaia_log::app().info("Applying sensor data from vertex {}", o.id()); map.apply_sensor_data(o); } @@ -137,15 +137,13 @@ void export_map_to_file() gaia::db::begin_transaction(); ego_t ego = *(ego_t::list().begin()); area_map_t& am = *(area_map_t::list().begin()); - world_coordinate_t top_left = { + world_coordinate_t bottom_left = { .x_meters = am.left_meters(), - .y_meters = am.top_meters() - }; - world_coordinate_t bottom_right = { - .x_meters = am.right_meters(), .y_meters = am.bottom_meters() }; - build_map(ego.current_graph(), top_left, bottom_right); + float width_meters = am.right_meters() - am.left_meters(); + float height_meters = am.top_meters() - am.bottom_meters(); + build_map(ego.current_graph(), bottom_left, width_meters, height_meters); gaia::db::commit_transaction(); } diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 45aa927..7596ab8 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -180,32 +180,50 @@ void seed_database(float x_meters, float y_meters) 0 // observation_id ); + // Area map and observed area start out with same dimensions. + float left = floorf(x_meters - (c_range_sensor_max_meters+1)); + float bottom = floorf(y_meters - (c_range_sensor_max_meters+1)); + float right = ceilf(x_meters + (c_range_sensor_max_meters+1)); + float top = ceilf(y_meters + (c_range_sensor_max_meters+1)); + gaia_id_t world_id = observed_area_t::insert_row( - floor(x_meters - (c_range_sensor_max_meters+1)), // left_meters - ceil(x_meters + (c_range_sensor_max_meters+1)), // right_meters - ceil(y_meters + (c_range_sensor_max_meters+1)), // top_meters - floor(y_meters - (c_range_sensor_max_meters+1)) // bottom_meters + left, // left_meters + right, // right_meters + top, // top_meters + bottom // bottom_meters ); + // Working map record. + // Create local context to not worry about variable name conflicts. gaia_id_t area_id = area_map_t::insert_row( - 0, // blob_id - floor(x_meters - (c_range_sensor_max_meters+1)), // left_meters - ceil(x_meters + (c_range_sensor_max_meters+1)), // right_meters - ceil(y_meters + (c_range_sensor_max_meters+1)), // top_meters - floor(y_meters - (c_range_sensor_max_meters+1)), // bottom_meters - 0, // num_rows - 0 // num_cols + 0, // blob_id + left, // left_meters + right, // right_meters + top, // top_meters + bottom, // bottom_meters + 0, // num_rows + 0 // num_cols ); + // Working map record. + // Working map has only one blob as it doesn't change size, which + // is id=1. + // Create an empty working map to get map dimensions from. + working_grid_t map; + world_coordinate_t bl = map.get_bottom_left(); + map_size_t size = map.get_map_size(); + grid_size_t dims = map.get_grid_size(); gaia_id_t working_id = working_map_t::insert_row( - 0, // blob_id - floor(x_meters - (c_range_sensor_max_meters)), // left_meters - ceil(x_meters + (c_range_sensor_max_meters)), // right_meters - ceil(y_meters + (c_range_sensor_max_meters)), // top_meters - floor(y_meters - (c_range_sensor_max_meters)), // bottom_meters - 0, // num_rows - 0 // num_cols + 1, // blob_id + bl.x_meters, // left_meters + bl.x_meters + size.x_meters, // right_meters + bl.y_meters + size.y_meters, // top_meters + bl.y_meters, // bottom_meters + dims.rows, // num_rows + dims.cols // num_cols ); + // Don't create blob here. Blob will be created and initialized + // when it's first needed. gaia_id_t destination_id = destination_t::insert_row( x_meters, // x_meters diff --git a/slam_sim/src/utils/blob_cache.cpp b/slam_sim/src/utils/blob_cache.cpp index 3da31b9..c6ac08c 100644 --- a/slam_sim/src/utils/blob_cache.cpp +++ b/slam_sim/src/utils/blob_cache.cpp @@ -9,7 +9,8 @@ #include "blob_cache.hpp" #include "retail_assert.hpp" -blob_cache_t blob_cache_t::s_blob_cache; +namespace slam_sim +{ blob_t::blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob) { @@ -47,12 +48,9 @@ void blob_t::reset(uint32_t id, size_t size, uint32_t id_superseded_blob) data = new uint8_t[size]; } -blob_cache_t* blob_cache_t::get() -{ - return &s_blob_cache; -} -blob_t* blob_cache_t::create_or_reset_blob(uint32_t id, size_t size, uint32_t id_superseded_blob) +blob_t* blob_cache_t::create_or_reset_blob(uint32_t id, size_t size, + uint32_t id_superseded_blob) { RETAIL_ASSERT( id != c_invalid_blob_id, @@ -87,19 +85,19 @@ blob_t* blob_cache_t::create_or_reset_blob(uint32_t id, size_t size, uint32_t id } // Check if we have a previously superseded blob to delete. - // We only need to do this for new blobs, because old blobs have already gone through this. - if (is_new_blob && superseded_blob_ptr && superseded_blob_ptr->id_superseded_blob != c_invalid_blob_id) + // We only need to do this for new blobs, because old blobs have + // already gone through this. + if ((is_new_blob && superseded_blob_ptr) && + (superseded_blob_ptr->id_superseded_blob != c_invalid_blob_id)) { - auto iterator = m_blob_map.find(superseded_blob_ptr->id_superseded_blob); - + auto iterator = + m_blob_map.find(superseded_blob_ptr->id_superseded_blob); RETAIL_ASSERT( iterator != m_blob_map.end(), "Failed to find the previously superseded blob!"); - delete iterator->second; m_blob_map.erase(iterator); } - return blob_ptr; } @@ -114,3 +112,6 @@ blob_t* blob_cache_t::get_blob(uint32_t id) auto iterator = m_blob_map.find(id); return (iterator == m_blob_map.end()) ? nullptr : iterator->second; } + +} // namespace slam_sim + From f9864928fef8744733ad4ebf41a76a86b39edc67 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 13 Apr 2022 08:14:37 -0700 Subject: [PATCH 14/30] Compiling and linking. 4 API calls to be filled out still --- slam_sim/gaia/slam.ddl | 14 ++--- slam_sim/gaia/slam.ruleset | 2 +- slam_sim/include/globals.hpp | 25 ++++++++ slam_sim/include/slam_sim.hpp | 12 +--- slam_sim/src/Makefile | 1 - slam_sim/src/main.cpp | 1 + slam_sim/src/navigation.cpp | 112 ---------------------------------- slam_sim/src/path_map.cpp | 4 +- slam_sim/src/rule_api.cpp | 65 +++++++++++++++++++- 9 files changed, 100 insertions(+), 136 deletions(-) create mode 100644 slam_sim/include/globals.hpp delete mode 100644 slam_sim/src/navigation.cpp diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index efc2d6d..bee3254 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -136,13 +136,13 @@ table working_map blob_id uint32, -- Bounding polygon - -- Uses world coordinates, with increasing X,Y being rightward/upward - width_meters float, - height_meters float, --- left_meters float, --- right_meters float, --- top_meters float, --- bottom_meters float, + -- Uses world coordinates, with increasing X,Y being rightward/upward. + -- Position is relative to bot, with bot in center. Position must be + -- updated whenever map regenerated. + left_meters float, + right_meters float, + top_meters float, + bottom_meters float, -- Grid size num_rows uint32, num_cols uint32, diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index e3d2a78..f856453 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -89,7 +89,7 @@ ruleset map_ruleset : serial_group() // area map. if (need_to_extend_map()) { - build_area_map(/area_map, /observed_area); + build_area_map(/destination, /area_map, /observed_area); } build_working_map(/destination, /area_map, /working_map); } diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp new file mode 100644 index 0000000..b1b27d5 --- /dev/null +++ b/slam_sim/include/globals.hpp @@ -0,0 +1,25 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +#pragma once + +#include "blob_cache.hpp" + +namespace slam_sim +{ + +// Flag to indicate app should exit. Normally this is 0. When it's time +// to quit it's set to 1. +extern int32_t g_quit; + +// Blob caches for area maps and working maps. +extern blob_cache_t g_area_blobs; +extern blob_cache_t g_working_blobs; + +} // namespace slam_sim + diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 951f986..f5ed2f8 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -24,21 +24,12 @@ #include "gaia_slam.h" -#include "blob_cache.hpp" #include "occupancy.hpp" #include "sensor_data.hpp" namespace slam_sim { -// Flag to indicate app should exit. Normally this is 0. When it's time -// to quit it's set to 1. -extern int32_t g_quit; - -// Blob caches for area maps and working maps. -extern blob_cache_t g_area_blobs; -extern blob_cache_t g_working_blobs; - //////////////////////////////////////////////// // Rules API // This is the interface that is expected to invoked by rules @@ -60,7 +51,8 @@ bool need_to_extend_map(); // Generates a low-res map off the known world and generates coarse path // info to the destination. // Stores output 'area_map' record. -void build_area_map(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); +void build_area_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, + gaia::slam::observed_area_t&); // Generates a higher-res map of the local area for local route planning // and collision avoidance, and uses the area map for the map's path diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index bb324bc..3acec95 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -17,7 +17,6 @@ LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl all: $(OBJS) cd utils && make $(CXX) $(OBJS) $(EXT_OBJS) -o $(TARGET) $(LIBS) - #cp $(TARGET) ../runtime/sim_shell run: ./$(TARGET) -m ../data/map.json -x -3.0 -y -3.0 diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 7cc728f..7672f72 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -20,6 +20,7 @@ #include "gaia_slam.h" +#include "globals.hpp" #include "slam_sim.hpp" diff --git a/slam_sim/src/navigation.cpp b/slam_sim/src/navigation.cpp deleted file mode 100644 index 8ac2c24..0000000 --- a/slam_sim/src/navigation.cpp +++ /dev/null @@ -1,112 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////////// -// -// Primary API for rules relating to navigation and error estimation. -// -// Maps are meant to provide a means for Alice to determine areas to -// explore, for navigating between locations, and for avoiding -// obstacles. -// -// Maps are generated from the output of the SLAM algorithm, using -// observation data corrected for estimated errors. -// -//////////////////////////////////////////////////////////////////////// - -#include - -#include "slam_sim.hpp" - -namespace slam_sim -{ - -using std::string; - -using gaia::slam::graphs_t; -using gaia::slam::edges_t; -using gaia::slam::vertices_t; - -using gaia::slam::area_map_t; -using gaia::slam::working_map_t; -//using gaia::slam::error_corrections_t; - -using gaia::slam::area_map_writer; -//using gaia::slam::working_map_writer; - - -// Mock function to calculate error and optimize graph. -void optimize_graph(graphs_t& graph) -{ - gaia_log::app().info("Calculating error (mock)"); - for (vertices_t v: graph.observations()) - { - // Just iterate through the observations in the graph to show - // how it's done. - gaia_log::app().info("Vertex {} at {},{}", v.id(), - v.position().pos_x_meters(), v.position().pos_y_meters()); - } - for (edges_t e: graph.edges()) - { - // Just iterate through the edges in the graph to show - // how it's done. - gaia_log::app().info("Edge connecting vertices {} and {}", - e.src().id(), e.dest().id()); - } - // When the graph is optimized, the position data for each observation - // is subject to being updated. -} - - -//void export_area_map() -//{ -// static int32_t ctr = 0; -// string fname("map_" + std::to_string(ctr) + ".pgm"); -// gaia_log::app().info("Building map {}", fname); -// g_area_map->export_as_pnm(fname); -// ctr++; -//} - - -// TODO move to rules_api.cpp -void build_area_map(destination_t& dest, area_map_t& am, - observed_area_t& bounds) -{ -#warning "Need to adapt constructor to accept new bounds" - // Each time we build an area map - occupancy_grid_t area_map(am, true); - for (graphs_t& g: graphs_t::list()) - { - for (vertex_t& v: g.vertices()) - { - gaia_log::app().info("Pulling sensor data from {}:{}", - g.id(), v.id()); - area_map->apply_sensor_data(v); - } - } - // Build paths to destination - area_map.trace_routes(dest); - // Update blob ID in database - area_map_writer writer = am.writer(); - writer.blob_id = area_map.blob_id(); - writer.update_row(); -// export_area_map(); -} - - -void build_working_map(destination_t& dest, working_map_t& wm) -{ - // TODO rebuild the working map - // In the meantime, just 'touch' the record by updating the - // change counter. -// working_map_writer writer = wm.writer(); -// writer.change_counter = wm.change_counter() + 1; -// writer.update_row(); -} - -} // namespace slam_sim diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 870bdf6..82fc87c 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -412,7 +412,7 @@ world_coordinate_t occupancy_grid_t::get_node_location( // Builds map to destination using parent map to set boundary conditions. // If destination is outside of map bounds, only parent map is used // for path finding. -void occupancy_grid_t::trace_routes(world_coordinate_t destination, +void occupancy_grid_t::trace_routes(world_coordinate_t& destination, occupancy_grid_t& parent_map) { clear(); @@ -463,7 +463,7 @@ void occupancy_grid_t::trace_routes(world_coordinate_t destination, } -void occupancy_grid_t::trace_routes(world_coordinate_t destination) +void occupancy_grid_t::trace_routes(world_coordinate_t& destination) { clear(); grid_index_t idx = get_node_index(destination.x_meters, diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 34b6943..c9f35b1 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -28,16 +28,19 @@ namespace slam_sim using std::string; -using gaia::slam::destination_t; using gaia::slam::edges_t; using gaia::slam::ego_t; -using gaia::slam::error_corrections_t; using gaia::slam::graphs_t; -using gaia::slam::observed_area_t; using gaia::slam::vertices_t; + using gaia::slam::area_map_t; +using gaia::slam::destination_t; +using gaia::slam::error_corrections_t; +using gaia::slam::observed_area_t; using gaia::slam::working_map_t; +using gaia::slam::area_map_writer; +using gaia::slam::working_map_writer; // Determine if a new graph optimization is necessary. // In a live example, this function would apply logic to determine if @@ -148,5 +151,61 @@ void export_map_to_file() } +void build_area_map(destination_t& dest, area_map_t& am, + observed_area_t& bounds) +{ + // Each time we build an area map + area_grid_t area_map(am, bounds); + for (graphs_t& g: graphs_t::list()) + { + for (vertices_t& v: g.vertices()) + { + gaia_log::app().info("Pulling sensor data from {}:{}", + g.id(), v.id()); + area_map.apply_sensor_data(v); + } + } + // Build paths to destination + world_coordinate_t pos = { + .x_meters = dest.x_meters(), + .y_meters = dest.y_meters() + }; + area_map.trace_routes(pos); + // Update blob ID in database + area_map_writer writer = am.writer(); + writer.blob_id = area_map.get_blob_id(); + writer.update_row(); +// export_area_map(); +} + +//////////////////////////////////////////////////////////////////////// +// YET TO FINISH + +void build_working_map(destination_t& dest, area_map_t& am, working_map_t& wm) +{ + // TODO rebuild the working map + // In the meantime, just 'touch' the record by updating the + // change counter. +// working_map_writer writer = wm.writer(); +// writer.change_counter = wm.change_counter() + 1; +// writer.update_row(); +} + +void full_stop() +{ + // TODO Take and observation and stop moving. +} + +void move_toward_destination(working_map_t& wm) +{ + // TODO Relocate to new position and take and observation. +} + +bool select_new_destination() +{ + // TODO Determine if it's time to change destinations. + return false; +} + } // namespace slam_sim From 6e8178eb92e912de258afa5d4f56b1df448e6c6c Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 13 Apr 2022 15:00:00 -0700 Subject: [PATCH 15/30] Compiles & links. Filling in parts for bot motion and record creation. --- slam_sim/gaia/slam.ddl | 56 +++++++------- slam_sim/gaia/slam.ruleset | 16 ++-- slam_sim/include/constants.hpp | 5 ++ slam_sim/include/globals.hpp | 7 +- slam_sim/include/occupancy.hpp | 67 ++++++++++------- slam_sim/include/slam_sim.hpp | 17 +++-- slam_sim/src/main.cpp | 13 ++-- slam_sim/src/occupancy.cpp | 133 ++++++++++++++++++++------------- slam_sim/src/rule_api.cpp | 108 ++++++++++++++++++++++---- slam_sim/src/support.cpp | 43 ++++++----- 10 files changed, 298 insertions(+), 167 deletions(-) diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index bee3254..75d75c8 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -31,7 +31,7 @@ database slam; -- Single record tables: -- ego Stores state of the bot. -- area_map Map of the known world. --- working_map Version of world map with recent sensor overlay. +---- working_map Version of world map with recent sensor overlay. -- destination Location that bot is moving to. -- observed_area Bounds of observed world. -- @@ -83,8 +83,8 @@ table ego world references observed_area, -- Map oriented. - low_res_map references area_map, - working_map references working_map + map references area_map +-- working_map references working_map ) @@ -123,34 +123,34 @@ table area_map num_rows uint32, num_cols uint32, - ego references ego, - working_map references working_map + ego references ego +-- working_map references working_map ) -table working_map -( - -- Reference to in-memory blob that stores 2D map. - -- Blob ID changes on each map update. - -- Working map must be seeded with a different blob_id than area map. - blob_id uint32, - - -- Bounding polygon - -- Uses world coordinates, with increasing X,Y being rightward/upward. - -- Position is relative to bot, with bot in center. Position must be - -- updated whenever map regenerated. - left_meters float, - right_meters float, - top_meters float, - bottom_meters float, - -- Grid size - num_rows uint32, - num_cols uint32, - - -- References - ego references ego, - area_map references area_map -) +--table working_map +--( +-- -- Reference to in-memory blob that stores 2D map. +-- -- Blob ID changes on each map update. +-- -- Working map must be seeded with a different blob_id than area map. +-- blob_id uint32, +-- +-- -- Bounding polygon +-- -- Uses world coordinates, with increasing X,Y being rightward/upward. +-- -- Position is relative to bot, with bot in center. Position must be +-- -- updated whenever map regenerated. +-- left_meters float, +-- right_meters float, +-- top_meters float, +-- bottom_meters float, +-- -- Grid size +-- num_rows uint32, +-- num_cols uint32, +-- +-- -- References +-- ego references ego, +-- area_map references area_map +--) table destination diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index f856453..f6e14ad 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -27,7 +27,7 @@ Rules map_ruleset serial_group on_insert(edges) If optimization required, do so and rebuild maps. - on_change(destination) Rebuild working map, move to destination. + on_change(destination) Rebuild area map, move toward new destination. on_insert(vertices) Extend area map if necessary. ruleset @@ -43,9 +43,9 @@ using slam_sim::optimize_graph; using slam_sim::need_to_extend_map; using slam_sim::build_area_map; -using slam_sim::build_working_map; +//using slam_sim::build_working_map; -using slam_sim::select_new_destination; +using slam_sim::reassess_destination; using slam_sim::move_toward_destination; using slam_sim::full_stop; @@ -69,7 +69,7 @@ ruleset map_ruleset : serial_group() // maps. This can be triggered in a separate rule or simply // done here, so do it here. build_area_map(/destination, /area_map, /observed_area); - build_working_map(/destination, /area_map, /working_map); +// build_working_map(/destination, /area_map, /working_map); } } @@ -77,8 +77,8 @@ ruleset map_ruleset : serial_group() // Destination changed. Update working map and start moving toward it. on_change(destination) { - build_working_map(/destination, /area_map, /working_map); - move_toward_destination(/working_map); + build_area_map(/destination, /area_map, /observed_area); + move_toward_destination(/destination, /area_map); } @@ -91,7 +91,7 @@ ruleset map_ruleset : serial_group() { build_area_map(/destination, /area_map, /observed_area); } - build_working_map(/destination, /area_map, /working_map); +// build_working_map(/destination, /area_map, /working_map); } } @@ -126,7 +126,7 @@ ruleset runtime_ruleset on_insert(vertices) { // Determine if it's time to change destinations. - select_new_destination(); + reassess_destination(); } on_insert(collision_event) diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index c2615a6..350e34f 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -33,5 +33,10 @@ constexpr float c_path_penalty_per_observation = 0.05f; // the directional vector from each node. The constant here is that X. constexpr uint32_t c_num_ancestors_for_direction = 5; +// Distance between each step. Keyframes are taken after several steps. +constexpr float c_step_meters = 2.0f * c_working_map_node_width_meters; + +constexpr uint32_t c_num_steps_between_keyframes = 5; + } // namespace slam_sim diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp index b1b27d5..0a88c75 100644 --- a/slam_sim/include/globals.hpp +++ b/slam_sim/include/globals.hpp @@ -9,6 +9,7 @@ #pragma once #include "blob_cache.hpp" +#include "occupancy.hpp" namespace slam_sim { @@ -19,7 +20,11 @@ extern int32_t g_quit; // Blob caches for area maps and working maps. extern blob_cache_t g_area_blobs; -extern blob_cache_t g_working_blobs; + +// Bot's position. This is known at the 'infrastructure' level (e.g., ROS) +// which is where position data is fed to the DB from. This is the current +// (estimated) position of the bot. +extern world_coordinate_t g_position; } // namespace slam_sim diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 9f9549e..604b56f 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -42,6 +42,7 @@ namespace slam_sim // different contexts. // Physical location in world space. +// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). struct world_coordinate_t { float x_meters; @@ -49,6 +50,7 @@ struct world_coordinate_t }; // Bounding area of map in world space. +// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). struct map_size_t { float x_meters; @@ -63,6 +65,7 @@ struct grid_size_t }; // Node location in grid. +// NOTE: this is in image coordinates (i.e., 0,0 is top left). struct grid_coordinate_t { uint32_t x; @@ -70,6 +73,7 @@ struct grid_coordinate_t }; // Distance from one grid node to another, in units of grid coordinates. +// NOTE: this is in image coordiantes (i.e., increasing x,y is down/right). struct node_offset_t { int32_t dx; @@ -150,6 +154,13 @@ class occupancy_grid_t // Creates new uncached map. occupancy_grid_t(float node_width_meters, world_coordinate_t bottom_left, float width_meters, float height_meters); + // Constructors to be called by rules. + // Loads existing map, if present (map will be blank if it's doesn't + // exist yet). + occupancy_grid_t(gaia::slam::area_map_t&); + // Purges existing map and rebuilds a new one, using observed area as + // bounds. + occupancy_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); ~occupancy_grid_t(); @@ -158,7 +169,14 @@ class occupancy_grid_t // Returns a reference to tne map node at the specified location. map_node_t& get_node(float x_meters, float y_meters); + map_node_t& get_node(grid_index_t idx); map_node_flags_t& get_node_flags(float x_meters, float y_meters); + map_node_flags_t& get_node_flags(grid_index_t idx); + + grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); + + // Returns the center of the grid node at the specified coordinates. + world_coordinate_t get_node_position(grid_coordinate_t& pos); // Apply sensor data to map from vertex. void apply_sensor_data(const gaia::slam::vertices_t&); @@ -197,7 +215,6 @@ class occupancy_grid_t // managed externally. void allocate_own_grid(); - grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); void apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters); void apply_flags(); @@ -254,30 +271,30 @@ class occupancy_grid_t }; -class area_grid_t : public occupancy_grid_t -{ -public: - // Constructors to be called by rules. - // Loads existing map, if present (map will be blank if it's doesn't - // exist yet). - area_grid_t(gaia::slam::area_map_t&); - // Purges existing map and rebuilds a new one, using observed area as - // bounds. - area_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); -}; - - -class working_grid_t : public occupancy_grid_t -{ -public: - // Creates a grid that's not associated w/ database record. - working_grid_t(); - - // Constructor to be called by rules. - // Loads existinig working map. If this is to be embedded in larger - // map, that must be done separately. - working_grid_t(gaia::slam::working_map_t&); -}; +//class area_grid_t : public occupancy_grid_t +//{ +//public: +// // Constructors to be called by rules. +// // Loads existing map, if present (map will be blank if it's doesn't +// // exist yet). +// area_grid_t(gaia::slam::area_map_t&); +// // Purges existing map and rebuilds a new one, using observed area as +// // bounds. +// area_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); +//}; +// +// +//class working_grid_t : public occupancy_grid_t +//{ +//public: +// // Creates a grid that's not associated w/ database record. +// working_grid_t(); +// +// // Constructor to be called by rules. +// // Loads existinig working map. If this is to be embedded in larger +// // map, that must be done separately. +// working_grid_t(gaia::slam::working_map_t&); +//}; } // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index f5ed2f8..6de3179 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -54,21 +54,22 @@ bool need_to_extend_map(); void build_area_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, gaia::slam::observed_area_t&); -// Generates a higher-res map of the local area for local route planning -// and collision avoidance, and uses the area map for the map's path -// boundary conditions. -// Stores output 'working_map' record. -void build_working_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, - gaia::slam::working_map_t&); +//// Generates a higher-res map of the local area for local route planning +//// and collision avoidance, and uses the area map for the map's path +//// boundary conditions. +//// Stores output 'working_map' record. +//void build_working_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, +// gaia::slam::working_map_t&); // Checks to see if it's time to select a new destination. If so, the // destination record is updated and the function returns 'true'. Otherwise // returns 'false'. -bool select_new_destination(); +bool reassess_destination(); // Instructs hardware layer to move toward destination. -void move_toward_destination(gaia::slam::working_map_t&); +void move_toward_destination(gaia::slam::destination_t&, + gaia::slam::area_map_t&); // Instructs bot to stop. An observation will be taken here. void full_stop(); diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 7672f72..4000540 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -34,8 +34,7 @@ static constexpr uint32_t c_rule_wait_millis = 100; // Globals for this namespace int32_t g_quit = 0; -float g_initial_x_meters = 0.0; -float g_initial_y_meters = 0.0; +world_coordinate_t g_position = { .x_meters = 0.0, .y_meters = 0.0 }; /** @@ -70,7 +69,7 @@ void clean_db() gaia::db::begin_transaction(); remove_all_rows(); remove_all_rows(); - remove_all_rows(); +// remove_all_rows(); remove_all_rows(); remove_all_rows(); remove_all_rows(); @@ -115,7 +114,7 @@ void parse_command_line(int argc, char** argv) case 'x': try { - g_initial_x_meters = std::stod(optarg, NULL); + g_position.x_meters = std::stod(optarg, NULL); } catch (std::invalid_argument& e) { @@ -126,7 +125,7 @@ void parse_command_line(int argc, char** argv) case 'y': try { - g_initial_y_meters = std::stod(optarg, NULL); + g_position.y_meters = std::stod(optarg, NULL); } catch (std::invalid_argument& e) { @@ -146,7 +145,7 @@ void parse_command_line(int argc, char** argv) usage(argc, argv); } gaia_log::app().info("Initial possition at {},{}", - g_initial_x_meters, g_initial_y_meters); + g_position.x_meters, g_position.y_meters); gaia_log::app().info("Loading world map {}", map_file); load_world_map(map_file); } @@ -157,7 +156,7 @@ void init_sim() // Seed database and then create first path. // Seeding function manages its own transaction. gaia_log::app().info("Seeding the database..."); - slam_sim::seed_database(g_initial_x_meters, g_initial_y_meters); + slam_sim::seed_database(g_position.x_meters, g_position.y_meters); } } // namespace slam_sim; diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index eec945c..8d7cfb0 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -38,7 +38,7 @@ using gaia::slam::positions_t; using gaia::slam::range_data_t; using gaia::slam::area_map_t;; -using gaia::slam::working_map_t;; +//using gaia::slam::working_map_t;; using gaia::slam::observed_area_t;; using gaia::slam::area_map_writer; @@ -172,6 +172,12 @@ map_node_t& occupancy_grid_t::get_node(float x_meters, float y_meters) } +map_node_t& occupancy_grid_t::get_node(grid_index_t idx) +{ + return m_grid[idx.idx]; +} + + map_node_flags_t& occupancy_grid_t::get_node_flags( float x_meters, float y_meters) { @@ -180,6 +186,24 @@ map_node_flags_t& occupancy_grid_t::get_node_flags( } +map_node_flags_t& occupancy_grid_t::get_node_flags(grid_index_t idx) +{ + return m_grid[idx.idx].flags; +} + + +world_coordinate_t occupancy_grid_t::get_node_position(grid_coordinate_t& pos) +{ + assert(pos.x < m_grid_size.cols); + assert(pos.y < m_grid_size.rows); + world_coordinate_t coord; + coord.x_meters = m_bottom_left.x_meters + + ((float) pos.x + 0.5f) * m_node_size_meters; + coord.y_meters = m_bottom_left.y_meters + m_map_size.y_meters + - (((float) pos.y + 0.5f) * m_node_size_meters); + return coord; +} + //////////////////////////////////////////////////////////////////////// // Applying sensor data to the map grids @@ -272,6 +296,7 @@ void occupancy_grid_t::apply_sensor_data(const vertices_t& obs) // Applying sensor data to the map grids //////////////////////////////////////////////////////////////////////// // Compute path weights +// See path_map.cpp //////////////////////////////////////////////////////////////////////// // Export map @@ -322,10 +347,10 @@ void occupancy_grid_t::export_as_pnm(string file_name) } //////////////////////////////////////////////////////////////////////// -// area grid subclass +// area map constructors // Loads existing map. -area_grid_t::area_grid_t(area_map_t& am) +occupancy_grid_t::occupancy_grid_t(area_map_t& am) { world_coordinate_t bottom_left = { .x_meters = am.left_meters(), @@ -360,7 +385,7 @@ area_grid_t::area_grid_t(area_map_t& am) // Overwrites existing cached area map. -area_grid_t::area_grid_t(area_map_t& am, observed_area_t& area) +occupancy_grid_t::occupancy_grid_t(area_map_t& am, observed_area_t& area) { world_coordinate_t bottom_left = { .x_meters = am.left_meters(), @@ -388,56 +413,56 @@ area_grid_t::area_grid_t(area_map_t& am, observed_area_t& area) writer.update_row(); } -//////////////////////////////////////////////////////////////////////// -// working grid subclass - -// Load existing working map. -working_grid_t::working_grid_t() -{ - - world_coordinate_t bottom_left = { - .x_meters = floorf(-c_range_sensor_max_meters), - .y_meters = floorf(-c_range_sensor_max_meters) - }; - float right_meters = ceilf(c_range_sensor_max_meters); - float top_meters = ceilf(c_range_sensor_max_meters); - init(c_working_map_node_width_meters, bottom_left, - right_meters - bottom_left.x_meters, - top_meters - bottom_left.y_meters); - allocate_own_grid(); -} - -// Load existing working map. -working_grid_t::working_grid_t(working_map_t& wm) -{ - world_coordinate_t bottom_left = { - .x_meters = wm.left_meters(), - .y_meters = wm.bottom_meters() - }; - init(c_working_map_node_width_meters, bottom_left, - wm.right_meters() - wm.left_meters(), - wm.top_meters() - wm.bottom_meters()); - assert(m_grid_size.rows = wm.num_rows()); - assert(m_grid_size.cols = wm.num_cols()); - // Recover memory from blob cache. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_blob_id = wm.blob_id(); - assert(m_blob_id != c_invalid_blob_id); - blob_t* blob = g_working_blobs.get_blob(m_blob_id); - if (blob == NULL) - { - // Blob doesn't exist yet. This means that the process was - // started up against an existing database. This shouldn't - // happen, but allow it. Create it. - gaia_log::app().warn("Area map with ID {} did not have cached " - "blob. Recreating one.", m_blob_id); - size_t sz = num_nodes * sizeof *m_grid; - blob = g_working_blobs.create_or_reset_blob(wm.blob_id(), sz); - // New grid. Nodes need initialization. - clear(); - } - m_grid = (map_node_t*) blob->data; -} +////////////////////////////////////////////////////////////////////////// +//// working grid subclass +// +//// Load existing working map. +//working_grid_t::working_grid_t() +//{ +// +// world_coordinate_t bottom_left = { +// .x_meters = floorf(-c_range_sensor_max_meters), +// .y_meters = floorf(-c_range_sensor_max_meters) +// }; +// float right_meters = ceilf(c_range_sensor_max_meters); +// float top_meters = ceilf(c_range_sensor_max_meters); +// init(c_working_map_node_width_meters, bottom_left, +// right_meters - bottom_left.x_meters, +// top_meters - bottom_left.y_meters); +// allocate_own_grid(); +//} +// +//// Load existing working map. +//working_grid_t::working_grid_t(working_map_t& wm) +//{ +// world_coordinate_t bottom_left = { +// .x_meters = wm.left_meters(), +// .y_meters = wm.bottom_meters() +// }; +// init(c_working_map_node_width_meters, bottom_left, +// wm.right_meters() - wm.left_meters(), +// wm.top_meters() - wm.bottom_meters()); +// assert(m_grid_size.rows = wm.num_rows()); +// assert(m_grid_size.cols = wm.num_cols()); +// // Recover memory from blob cache. +// uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; +// m_blob_id = wm.blob_id(); +// assert(m_blob_id != c_invalid_blob_id); +// blob_t* blob = g_working_blobs.get_blob(m_blob_id); +// if (blob == NULL) +// { +// // Blob doesn't exist yet. This means that the process was +// // started up against an existing database. This shouldn't +// // happen, but allow it. Create it. +// gaia_log::app().warn("Area map with ID {} did not have cached " +// "blob. Recreating one.", m_blob_id); +// size_t sz = num_nodes * sizeof *m_grid; +// blob = g_working_blobs.create_or_reset_blob(wm.blob_id(), sz); +// // New grid. Nodes need initialization. +// clear(); +// } +// m_grid = (map_node_t*) blob->data; +//} } // namespace slam_sim diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index c9f35b1..6fcc0da 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -20,6 +20,7 @@ #include "gaia_slam.h" #include "constants.hpp" +#include "globals.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" @@ -37,10 +38,10 @@ using gaia::slam::area_map_t; using gaia::slam::destination_t; using gaia::slam::error_corrections_t; using gaia::slam::observed_area_t; -using gaia::slam::working_map_t; +//using gaia::slam::working_map_t; using gaia::slam::area_map_writer; -using gaia::slam::working_map_writer; +//using gaia::slam::working_map_writer; // Determine if a new graph optimization is necessary. // In a live example, this function would apply logic to determine if @@ -155,7 +156,7 @@ void build_area_map(destination_t& dest, area_map_t& am, observed_area_t& bounds) { // Each time we build an area map - area_grid_t area_map(am, bounds); + occupancy_grid_t area_map(am, bounds); for (graphs_t& g: graphs_t::list()) { for (vertices_t& v: g.vertices()) @@ -181,27 +182,106 @@ void build_area_map(destination_t& dest, area_map_t& am, //////////////////////////////////////////////////////////////////////// // YET TO FINISH -void build_working_map(destination_t& dest, area_map_t& am, working_map_t& wm) +void build_working_map(occupancy_grid_t& working_map, + destination_t& dest, area_map_t& am) { - // TODO rebuild the working map - // In the meantime, just 'touch' the record by updating the - // change counter. -// working_map_writer writer = wm.writer(); -// writer.change_counter = wm.change_counter() + 1; -// writer.update_row(); + occupancy_grid_t area_grid(am); + // Set boundary conditions. + // Iterate through boundary nodes of grid. Look up path values + // from area map. + grid_size_t sz = working_map.get_grid_size(); + for (uint32_t y=0; y Date: Tue, 19 Apr 2022 14:44:35 -0700 Subject: [PATCH 16/30] feature complete, untested --- slam_sim/data/map.json | 9 +- slam_sim/gaia/slam.ddl | 27 +-- slam_sim/gaia/slam.ruleset | 16 +- slam_sim/include/constants.hpp | 3 + slam_sim/include/globals.hpp | 9 + slam_sim/include/slam_sim.hpp | 16 +- slam_sim/src/analyze.cpp | 15 ++ slam_sim/src/main.cpp | 6 +- slam_sim/src/occupancy.cpp | 2 + slam_sim/src/rule_api.cpp | 85 +++++----- slam_sim/src/support.cpp | 294 +++++++++++++++++---------------- 11 files changed, 248 insertions(+), 234 deletions(-) diff --git a/slam_sim/data/map.json b/slam_sim/data/map.json index 46a26f2..03bb833 100644 --- a/slam_sim/data/map.json +++ b/slam_sim/data/map.json @@ -59,11 +59,8 @@ ] } ], - "landmarks": [ - { "x": -3.5, "y": 4.9, "id": 1, "name": "mark-1" }, - { "x": 3.5, "y": 4.9, "id": 2, "name": "mark-2" }, - { "x": -3.5, "y": -4.9, "id": 2, "name": "mark-3" }, - { "x": 3.5, "y": -4.9, "id": 2, "name": "mark-4" }, - { "x": 6.0, "y": -0.0, "id": 2, "name": "mark-5" } + "destinations": [ + { "x": 5.0, "y": 0.5 }, + { "x": -5.7, "y": 4.1 } ] } diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 75d75c8..d7cccec 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -128,31 +128,6 @@ table area_map ) ---table working_map ---( --- -- Reference to in-memory blob that stores 2D map. --- -- Blob ID changes on each map update. --- -- Working map must be seeded with a different blob_id than area map. --- blob_id uint32, --- --- -- Bounding polygon --- -- Uses world coordinates, with increasing X,Y being rightward/upward. --- -- Position is relative to bot, with bot in center. Position must be --- -- updated whenever map regenerated. --- left_meters float, --- right_meters float, --- top_meters float, --- bottom_meters float, --- -- Grid size --- num_rows uint32, --- num_cols uint32, --- --- -- References --- ego references ego, --- area_map references area_map ---) - - table destination ( -- Uses world coordinates, with increasing X,Y being rightward/upward. @@ -215,7 +190,7 @@ table graphs vertices references vertices[], edges references edges[], - -- Reverse reference to ego (necessary until one-way references are + -- Reverse reference (VLR) to ego (necessary until one-way references are -- supported) ego references ego[] ) diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index f6e14ad..25e5a35 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -64,12 +64,10 @@ ruleset map_ruleset : serial_group() if (optimization_required()) { optimize_graph(edges.dest->vertices.graph->graphs); - // Whenever new error-correction data is available, regenerate // maps. This can be triggered in a separate rule or simply // done here, so do it here. build_area_map(/destination, /area_map, /observed_area); -// build_working_map(/destination, /area_map, /working_map); } } @@ -91,7 +89,6 @@ ruleset map_ruleset : serial_group() { build_area_map(/destination, /area_map, /observed_area); } -// build_working_map(/destination, /area_map, /working_map); } } @@ -120,13 +117,17 @@ ruleset runtime_ruleset on_insert(vertices) { // TODO Perform spatial query in area of observation and see if - // there are other vertices to build edges with. + // there are other vertices to build edges with (i.e., build + // loop closures) } on_insert(vertices) { // Determine if it's time to change destinations. - reassess_destination(); + if (reassess_destination()) + { + // If destination changes, start a new graph. + } } on_insert(collision_event) @@ -137,5 +138,10 @@ ruleset runtime_ruleset gaia_log::app().info("Collision detected. Stopping bot"); full_stop(); } + + on_insert(graphs) + { + /ego.current_graph_id = graphs.id + } } diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 350e34f..1a150a9 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -38,5 +38,8 @@ constexpr float c_step_meters = 2.0f * c_working_map_node_width_meters; constexpr uint32_t c_num_steps_between_keyframes = 5; +// How close to destination bot has to declare it's reached it. +constexpr float c_destination_radius_meters = 0.5f; + } // namespace slam_sim diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp index 0a88c75..1126dad 100644 --- a/slam_sim/include/globals.hpp +++ b/slam_sim/include/globals.hpp @@ -8,6 +8,8 @@ #pragma once +#include + #include "blob_cache.hpp" #include "occupancy.hpp" @@ -25,6 +27,13 @@ extern blob_cache_t g_area_blobs; // which is where position data is fed to the DB from. This is the current // (estimated) position of the bot. extern world_coordinate_t g_position; +extern float g_heading_degs; + +extern std::vector g_destinations; +extern uint32_t g_next_destination; + +// Current simulation time. +extern double g_now; } // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 6de3179..544a73b 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -54,14 +54,6 @@ bool need_to_extend_map(); void build_area_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, gaia::slam::observed_area_t&); -//// Generates a higher-res map of the local area for local route planning -//// and collision avoidance, and uses the area map for the map's path -//// boundary conditions. -//// Stores output 'working_map' record. -//void build_working_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, -// gaia::slam::working_map_t&); - - // Checks to see if it's time to select a new destination. If so, the // destination record is updated and the function returns 'true'. Otherwise // returns 'false'. @@ -80,7 +72,7 @@ void full_stop(); // Creates a record in the vertices table using the supplied sensor // data -void create_vertex(const slam_sim::sensor_data_t& data); +void create_vertex(world_coordinate_t pos, float heading_degs); // Do a sensor sweep from at the stated position. This would normally be // pushed up from the sensor/hardware layer, but polling for it makes @@ -96,6 +88,12 @@ void export_map_to_file(); void request_destination(float x_meters, float y_meters); +//////////////////////////////////////////////// +// Environmental analysis + +// Calculates sensor view from a position in the environment. +void calculate_range_data(map_coord_t& coord, sensor_data_t& data); + //////////////////////////////////////////////// // Initialization diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index e662dfd..ec558b0 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -23,6 +23,7 @@ #include "constants.hpp" #include "json.hpp" +#include "globals.hpp" #include "line_segment.hpp" #include "sensor_data.hpp" #include "slam_sim.hpp" @@ -59,6 +60,20 @@ static void set_map(const char* map) g_world_lines.push_back(line_segment_t(x0, y0, x1, y1)); } } + const json& destinations = world_map["destinations"]; + for (uint32_t i=0; i g_destinations; +uint32_t g_next_destination = 0; + +double g_now = 0.0; /** * Wait for simulation to complete. @@ -69,7 +74,6 @@ void clean_db() gaia::db::begin_transaction(); remove_all_rows(); remove_all_rows(); -// remove_all_rows(); remove_all_rows(); remove_all_rows(); remove_all_rows(); diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 8d7cfb0..2cd281f 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -281,6 +281,8 @@ void occupancy_grid_t::apply_flags() void occupancy_grid_t::apply_sensor_data(const vertices_t& obs) { + // TODO make sure that sensor data from outside of map is ignored. + // positions_t pos = obs.position(); range_data_t r = obs.range_data(); //printf("Applying sensor data at %.2f,%.2f (%d)\n", pos.pos_x_meters(), pos.pos_y_meters(), obs.id()); diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 6fcc0da..1cfec97 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -38,10 +38,9 @@ using gaia::slam::area_map_t; using gaia::slam::destination_t; using gaia::slam::error_corrections_t; using gaia::slam::observed_area_t; -//using gaia::slam::working_map_t; using gaia::slam::area_map_writer; -//using gaia::slam::working_map_writer; +using gaia::slam::destination_writer; // Determine if a new graph optimization is necessary. // In a live example, this function would apply logic to determine if @@ -179,56 +178,27 @@ void build_area_map(destination_t& dest, area_map_t& am, // export_area_map(); } -//////////////////////////////////////////////////////////////////////// -// YET TO FINISH void build_working_map(occupancy_grid_t& working_map, destination_t& dest, area_map_t& am) { - occupancy_grid_t area_grid(am); - // Set boundary conditions. - // Iterate through boundary nodes of grid. Look up path values - // from area map. - grid_size_t sz = working_map.get_grid_size(); - for (uint32_t y=0; y= g_destinations.size()) + { + g_next_destination = 0; + } + destination_writer w = dest.writer(); + w.x_meters = new_dest.x_meters; + w.y_meters = new_dest.y_meters; + w.update_row(); + g_now += 1.0; + return true; + } return false; } diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 2be54f1..981f2b9 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -21,6 +21,7 @@ #include #include "constants.hpp" +#include "globals.hpp" #include "slam_sim.hpp" //#include "line_segment.hpp" @@ -28,136 +29,166 @@ namespace slam_sim { -//static int32_t s_next_observation_id = 1; +static int32_t s_next_vertex_id = 1; + // using gaia::common::gaia_id_t; // using gaia::slam::area_map_t; -//using gaia::slam::edges_t; +using gaia::slam::edges_t; using gaia::slam::destination_t; using gaia::slam::ego_t; using gaia::slam::graphs_t; using gaia::slam::latest_observation_t; -//using gaia::slam::movements_t; -//using gaia::slam::observations_t; -using gaia::slam::observed_area_t; +using gaia::slam::movements_t; using gaia::slam::observed_area_t; -//using gaia::slam::positions_t; -//using gaia::slam::range_data_t; -// -//using gaia::slam::latest_observation_writer; -//using gaia::slam::observed_area_writer; -// -// -////////////////////////////////////////////////////////////////////////// -//// Background API that supports the contents of rule_api.cpp. Most -//// importantly, this means that the functions here are expected to -//// be called from within an active transaction -// -//static void update_observed_area(ego_t& ego, map_coord_t coord) -//{ -// observed_area_t area = ego.world(); -// bool change = false; -// // Left and right bounds. -// float left_edge = area.left_meters(); -// float right_edge = area.right_meters(); -// if (floor(coord.x_meters - (c_range_sensor_max_meters+1)) < left_edge) -// { -// left_edge = floor(coord.x_meters - (c_range_sensor_max_meters+1)); -// change = true; -// } -// if (ceil(coord.x_meters + (c_range_sensor_max_meters+1)) > right_edge) -// { -// right_edge = floor(coord.x_meters + (c_range_sensor_max_meters+1)); -// change = true; -// } -// // Top and bottom bounds. -// float bottom_edge = area.bottom_meters(); -// float top_edge = area.top_meters(); -// if (floor(coord.y_meters - (c_range_sensor_max_meters+1)) < bottom_edge) -// { -// bottom_edge = floor(coord.y_meters - (c_range_sensor_max_meters+1)); -// change = true; -// } -// if (ceil(coord.y_meters + (c_range_sensor_max_meters+1)) > top_edge) -// { -// top_edge = floor(coord.y_meters + (c_range_sensor_max_meters+1)); -// change = true; -// } -// // If bounds were modified, update observed area record. -// if (change) -// { -// observed_area_writer oa_writer = area.writer(); -// oa_writer.left_meters = left_edge; -// oa_writer.right_meters = right_edge; -// oa_writer.bottom_meters = bottom_edge; -// oa_writer.top_meters = top_edge; -// oa_writer.update_row(); -// } -//} -// -//void create_observation(map_coord_t& prev, map_coord_t& coord) -//{ -// uint32_t obs_num = s_next_observation_id++; -// gaia_log::app().info("Performing observation {} at {},{} heading {}", -// obs_num, coord.x_meters, coord.y_meters, coord.heading_degs); -// sensor_data_t data; -// calculate_range_data(coord, data); -// -// // Get ego -// ego_t& ego = *(ego_t::list().begin()); -// uint32_t graph_id = ego.current_graph_id(); -// -// // create position record -// gaia_id_t pos_id = positions_t::insert_row( -// coord.x_meters, // x_meters -// coord.y_meters, // y_meters -// coord.heading_degs // heading_degs -// ); -// // create range_data record -// gaia_id_t range_id = range_data_t::insert_row( -// data.num_radials, // num_radials -// data.bearing_degs, // bearing_degs -// data.range_meters // distance_meters -// ); -// // create movement record -// gaia_id_t movement_id = movements_t::insert_row( -// coord.x_meters - prev.x_meters, // dx_meters -// coord.y_meters - prev.y_meters, // dy_meters -// coord.heading_degs - prev.heading_degs // dheading_degs -// ); -// // create observation record -// gaia_id_t observation_id = observations_t::insert_row( -// obs_num, // id -// graph_id // graph_id -// ); -// -// // create references -// observations_t obs = observations_t::get(observation_id); -// obs.position().connect(pos_id); -// obs.range_data().connect(range_id); -// obs.motion().connect(movement_id); -// -// update_observed_area(ego, coord); -// -// // create edge -// latest_observation_t lo = ego.latest_observation(); -// observations_t prev_obs = lo.observation(); -// if (prev_obs) -// { -// edges_t::insert_row( -// graph_id, // graph_id -// prev_obs.id(), // src_id -// obs_num // dest_id -// ); -// } -// -// // update latest_observation -// latest_observation_writer lo_writer = lo.writer(); -// lo_writer.observation_id = obs_num; -// lo_writer.update_row(); -//} -// +using gaia::slam::positions_t; +using gaia::slam::range_data_t; +using gaia::slam::vertices_t; + +using gaia::slam::latest_observation_writer; +using gaia::slam::observed_area_writer; + + +//////////////////////////////////////////////////////////////////////// +// Background API that supports the contents of rule_api.cpp. Most +// importantly, this means that the functions here are expected to +// be called from within an active transaction + +static void update_observed_area(ego_t& ego) +{ + observed_area_t area = ego.world(); + bool change = false; + // Left and right bounds. + float left_edge = area.left_meters(); + float right_edge = area.right_meters(); + float max_range = c_range_sensor_max_meters; + if (floor(g_position.x_meters - max_range) < left_edge) + { + left_edge = floor(g_position.x_meters - max_range); + change = true; + } + if (ceil(g_position.x_meters + max_range) > right_edge) + { + right_edge = floor(g_position.x_meters + max_range); + change = true; + } + // Top and bottom bounds. + float bottom_edge = area.bottom_meters(); + float top_edge = area.top_meters(); + if (floor(g_position.y_meters - max_range) < bottom_edge) + { + bottom_edge = floor(g_position.y_meters - max_range); + change = true; + } + if (ceil(g_position.y_meters + max_range) > top_edge) + { + top_edge = floor(g_position.y_meters + max_range); + change = true; + } + // If bounds were modified, update observed area record. + if (change) + { + observed_area_writer oa_writer = area.writer(); + oa_writer.left_meters = left_edge; + oa_writer.right_meters = right_edge; + oa_writer.bottom_meters = bottom_edge; + oa_writer.top_meters = top_edge; + oa_writer.update_row(); + } +} + + +void create_vertex(world_coordinate_t pos, float heading_degs) +{ + uint32_t obs_num = s_next_vertex_id++; + gaia_log::app().info("Performing observation {} at {},{} heading {}", + obs_num, pos.x_meters, pos.y_meters, heading_degs); + sensor_data_t data; + map_coord_t coord = { + .x_meters = pos.x_meters, + .y_meters = pos.y_meters, + .heading_degs = heading_degs + }; + calculate_range_data(coord, data); + + // Get ego + ego_t& ego = *(ego_t::list().begin()); + uint32_t graph_id = ego.current_graph_id(); + + // Get most recent obesrvation. + vertices_t prev_vert = ego.latest_observation().vertex(); + positions_t prev_pos = prev_vert.position(); + float prev_x_meters, prev_y_meters, prev_heading_degs; + if (prev_pos) + { + prev_x_meters = prev_pos.x_meters(); + prev_y_meters = prev_pos.y_meters(); + prev_heading_degs = prev_pos.heading_degs(); + } + else + { + prev_x_meters = pos.x_meters; + prev_y_meters = pos.y_meters; + prev_heading_degs = heading_degs; + } + + gaia_id_t vertex_id = vertices_t::insert_row( + obs_num, // id + graph_id, // graph_id + g_now // timestamp + ); + + // create position record + gaia_id_t pos_id = positions_t::insert_row( + pos.x_meters, // x_meters + pos.y_meters, // y_meters + heading_degs // heading_degs + ); + + // create range_data record + gaia_id_t range_id = range_data_t::insert_row( + data.num_radials, // num_radials + data.bearing_degs, // bearing_degs + data.range_meters // distance_meters + ); + + // create movement record + float d_degs = heading_degs - prev_heading_degs; + if (d_degs < 0.0f) + { + d_degs += 360.0f; + } + gaia_id_t movement_id = movements_t::insert_row( + g_position.x_meters - prev_x_meters, // dx_meters + g_position.y_meters - prev_y_meters, // dy_meters + d_degs // dheading_degs + ); + + // create references + vertices_t v = vertices_t::get(vertex_id); + v.position().connect(pos_id); + v.range_data().connect(range_id); + v.motion().connect(movement_id); + + update_observed_area(ego); + + // create edge + if (prev_vert) + { + edges_t::insert_row( + graph_id, // graph_id + prev_vert.id(), // src_id + obs_num // dest_id + ); + } + + // update latest_observation + latest_observation_writer lo_writer = ego.latest_observation().writer(); + lo_writer.vertex_id = obs_num; + lo_writer.update_row(); +} + //////////////////////////////////////////////////////////////////////// // Non-rule API @@ -204,26 +235,6 @@ void seed_database(float x_meters, float y_meters) 0 // num_cols ); -// // Working map record. -// // Working map has only one blob as it doesn't change size, which -// // is id=1. -// // Create an empty working map to get map dimensions from. -// working_grid_t map; -// world_coordinate_t bl = map.get_bottom_left(); -// map_size_t size = map.get_map_size(); -// grid_size_t dims = map.get_grid_size(); -// gaia_id_t working_id = working_map_t::insert_row( -// 1, // blob_id -// bl.x_meters, // left_meters -// bl.x_meters + size.x_meters, // right_meters -// bl.y_meters + size.y_meters, // top_meters -// bl.y_meters, // bottom_meters -// dims.rows, // num_rows -// dims.cols // num_cols -// ); -// // Don't create blob here. Blob will be created and initialized -// // when it's first needed. - gaia_id_t destination_id = destination_t::insert_row( x_meters, // x_meters y_meters, // y_meters @@ -236,7 +247,6 @@ void seed_database(float x_meters, float y_meters) ego.world().connect(world_id); ego.latest_observation().connect(latest_observation_id); ego.map().connect(area_id); -// ego.working_map().connect(working_id); //////////////////////////////////////////// // All done From e210d2bd4017ddc28504f8df8724c3e756300f24 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Wed, 20 Apr 2022 10:59:52 -0700 Subject: [PATCH 17/30] fixed deadlock error in blob cache --- slam_sim/include/blob_cache.hpp | 4 ++++ slam_sim/src/utils/blob_cache.cpp | 14 ++++++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp index 0abff9c..4c01516 100644 --- a/slam_sim/include/blob_cache.hpp +++ b/slam_sim/include/blob_cache.hpp @@ -87,6 +87,10 @@ class blob_cache_t // Map of allocated blobs. std::unordered_map m_blob_map; std::shared_mutex m_lock; + + // Nested get_blob() call from w/in create_or_reset_blob(), where a + // lock has already been established. + blob_t* get_blob_locked(uint32_t id); }; } // namespace slam_sim diff --git a/slam_sim/src/utils/blob_cache.cpp b/slam_sim/src/utils/blob_cache.cpp index c6ac08c..5aa8605 100644 --- a/slam_sim/src/utils/blob_cache.cpp +++ b/slam_sim/src/utils/blob_cache.cpp @@ -65,14 +65,14 @@ blob_t* blob_cache_t::create_or_reset_blob(uint32_t id, size_t size, blob_t* superseded_blob_ptr = nullptr; if (id_superseded_blob != c_invalid_blob_id) { - superseded_blob_ptr = get_blob(id_superseded_blob); + superseded_blob_ptr = get_blob_locked(id_superseded_blob); } // Check if a blob with this id already exists; // if it exists, then reuse it; // if not, then create a new one and insert it in our map. bool is_new_blob = false; - blob_t* blob_ptr = get_blob(id); + blob_t* blob_ptr = get_blob_locked(id); if (blob_ptr) { blob_ptr->reset(id, size, id_superseded_blob); @@ -113,5 +113,15 @@ blob_t* blob_cache_t::get_blob(uint32_t id) return (iterator == m_blob_map.end()) ? nullptr : iterator->second; } +blob_t* blob_cache_t::get_blob_locked(uint32_t id) +{ + RETAIL_ASSERT( + id != c_invalid_blob_id, + "Attempting to retrieve a blob using an invalid id!"); + + auto iterator = m_blob_map.find(id); + return (iterator == m_blob_map.end()) ? nullptr : iterator->second; +} + } // namespace slam_sim From 2cb5a902a408848f631acd13ba723ff6c75ffcf1 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Thu, 21 Apr 2022 13:11:14 -0700 Subject: [PATCH 18/30] misc fixes --- slam_sim/gaia/slam.ruleset | 2 +- slam_sim/include/blob_cache.hpp | 4 +++ slam_sim/src/Makefile | 3 ++ slam_sim/src/occupancy.cpp | 57 ++----------------------------- slam_sim/src/path_map.cpp | 14 ++++---- slam_sim/src/rule_api.cpp | 1 - slam_sim/src/support.cpp | 2 ++ slam_sim/src/utils/blob_cache.cpp | 14 ++++++-- 8 files changed, 32 insertions(+), 65 deletions(-) diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 25e5a35..98d5f27 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -141,7 +141,7 @@ ruleset runtime_ruleset on_insert(graphs) { - /ego.current_graph_id = graphs.id + /ego.current_graph_id = graphs.id; } } diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp index 0abff9c..4c01516 100644 --- a/slam_sim/include/blob_cache.hpp +++ b/slam_sim/include/blob_cache.hpp @@ -87,6 +87,10 @@ class blob_cache_t // Map of allocated blobs. std::unordered_map m_blob_map; std::shared_mutex m_lock; + + // Nested get_blob() call from w/in create_or_reset_blob(), where a + // lock has already been established. + blob_t* get_blob_locked(uint32_t id); }; } // namespace slam_sim diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 3acec95..43174b5 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -21,6 +21,9 @@ all: $(OBJS) run: ./$(TARGET) -m ../data/map.json -x -3.0 -y -3.0 +drun: + gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y -3.0 + %.o: %.cpp $(CXX) $(INC) -c $(CPPFLAGS) $< diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 2cd281f..ec9f9e5 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -361,12 +361,9 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am) init(c_area_map_node_width_meters, bottom_left, am.right_meters() - am.left_meters(), am.top_meters() - am.bottom_meters()); - assert(m_grid_size.rows = am.num_rows()); - assert(m_grid_size.cols = am.num_cols()); // Recover memory from blob cache. uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; m_blob_id = am.blob_id();; - assert(m_blob_id != c_invalid_blob_id); blob_t* blob = NULL; // Fetch existing blob. g_area_blobs.get_blob(m_blob_id); @@ -381,6 +378,9 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am) blob = g_area_blobs.create_or_reset_blob(m_blob_id, sz); // New grid. Nodes need initialization. clear(); + } else { + assert(m_grid_size.rows == am.num_rows()); + assert(m_grid_size.cols == am.num_cols()); } m_grid = (map_node_t*) blob->data; } @@ -415,56 +415,5 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am, observed_area_t& area) writer.update_row(); } -////////////////////////////////////////////////////////////////////////// -//// working grid subclass -// -//// Load existing working map. -//working_grid_t::working_grid_t() -//{ -// -// world_coordinate_t bottom_left = { -// .x_meters = floorf(-c_range_sensor_max_meters), -// .y_meters = floorf(-c_range_sensor_max_meters) -// }; -// float right_meters = ceilf(c_range_sensor_max_meters); -// float top_meters = ceilf(c_range_sensor_max_meters); -// init(c_working_map_node_width_meters, bottom_left, -// right_meters - bottom_left.x_meters, -// top_meters - bottom_left.y_meters); -// allocate_own_grid(); -//} -// -//// Load existing working map. -//working_grid_t::working_grid_t(working_map_t& wm) -//{ -// world_coordinate_t bottom_left = { -// .x_meters = wm.left_meters(), -// .y_meters = wm.bottom_meters() -// }; -// init(c_working_map_node_width_meters, bottom_left, -// wm.right_meters() - wm.left_meters(), -// wm.top_meters() - wm.bottom_meters()); -// assert(m_grid_size.rows = wm.num_rows()); -// assert(m_grid_size.cols = wm.num_cols()); -// // Recover memory from blob cache. -// uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; -// m_blob_id = wm.blob_id(); -// assert(m_blob_id != c_invalid_blob_id); -// blob_t* blob = g_working_blobs.get_blob(m_blob_id); -// if (blob == NULL) -// { -// // Blob doesn't exist yet. This means that the process was -// // started up against an existing database. This shouldn't -// // happen, but allow it. Create it. -// gaia_log::app().warn("Area map with ID {} did not have cached " -// "blob. Recreating one.", m_blob_id); -// size_t sz = num_nodes * sizeof *m_grid; -// blob = g_working_blobs.create_or_reset_blob(wm.blob_id(), sz); -// // New grid. Nodes need initialization. -// clear(); -// } -// m_grid = (map_node_t*) blob->data; -//} - } // namespace slam_sim diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 82fc87c..f1ce5c1 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -154,7 +154,7 @@ void occupancy_grid_t::add_node_to_stack( { // Make sure we're not going off the edge of the map. If so, ignore. assert(root_idx.idx >= 0); - map_node_t& root_node = m_grid[root_idx.idx]; + const map_node_t& root_node = m_grid[root_idx.idx]; int32_t new_x = (int32_t) root_node.pos.x + offset.dx; int32_t new_y = (int32_t) root_node.pos.y + offset.dy; if ((new_x < 0) || (new_x >= m_grid_size.cols) || (new_y < 0) || @@ -214,7 +214,7 @@ void occupancy_grid_t::add_node_to_stack_diag( { // make sure we're not going off the edge of the map assert(root_idx.idx >= 0); - map_node_t& root_node = m_grid[root_idx.idx]; + const map_node_t& root_node = m_grid[root_idx.idx]; int32_t new_x = (int32_t) root_node.pos.x + offset.dx; int32_t new_y = (int32_t) root_node.pos.y + offset.dy; if ((new_x < 0) || (new_x >= m_grid_size.cols) @@ -225,7 +225,7 @@ void occupancy_grid_t::add_node_to_stack_diag( ///////////////////////////////////////////////////////////////////// // point is in the world -- check it grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; - map_node_t& child_node = m_grid[new_idx.idx]; + const map_node_t& child_node = m_grid[new_idx.idx]; if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { return; @@ -237,8 +237,8 @@ void occupancy_grid_t::add_node_to_stack_diag( (uint32_t) (root_node.pos.x + new_y * m_grid_size.cols); uint32_t idx_horiz = (uint32_t) (new_x + root_node.pos.y * m_grid_size.cols); - map_node_t& vert_child_node = m_grid[idx_vert]; - map_node_t& horiz_child_node = m_grid[idx_horiz]; + const map_node_t& vert_child_node = m_grid[idx_vert]; + const map_node_t& horiz_child_node = m_grid[idx_horiz]; if ((vert_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) && (horiz_child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE)) { @@ -330,12 +330,12 @@ void occupancy_grid_t::compute_path_costs() for (uint32_t x=0; xreset(id, size, id_superseded_blob); @@ -113,5 +113,15 @@ blob_t* blob_cache_t::get_blob(uint32_t id) return (iterator == m_blob_map.end()) ? nullptr : iterator->second; } +blob_t* blob_cache_t::get_blob_locked(uint32_t id) +{ + RETAIL_ASSERT( + id != c_invalid_blob_id, + "Attempting to retrieve a blob using an invalid id!"); + + auto iterator = m_blob_map.find(id); + return (iterator == m_blob_map.end()) ? nullptr : iterator->second; +} + } // namespace slam_sim From b1c4c0101fd5f83e8f2fd1a362751639e45ea32b Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Tue, 26 Apr 2022 20:17:33 -0700 Subject: [PATCH 19/30] bot moving and exporting buggy map --- slam_sim/.gitignore | 1 + slam_sim/Makefile | 11 ++++--- slam_sim/gaia/slam.ruleset | 14 ++++++++- slam_sim/include/sensor_data.hpp | 2 +- slam_sim/include/slam_sim.hpp | 3 +- slam_sim/src/analyze.cpp | 7 +++-- slam_sim/src/occupancy.cpp | 13 ++++----- slam_sim/src/rule_api.cpp | 50 +++++++++++++++++++++++++++----- slam_sim/src/support.cpp | 8 +++-- 9 files changed, 80 insertions(+), 29 deletions(-) diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index 80a8978..8964162 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -5,3 +5,4 @@ out2 slam_sim *.o logs +*.pgm diff --git a/slam_sim/Makefile b/slam_sim/Makefile index 9344fbf..f165e58 100644 --- a/slam_sim/Makefile +++ b/slam_sim/Makefile @@ -1,18 +1,17 @@ all: - #mkdir -p build && cd build && cmake .. && make -j 4 + cd gaia && make cd src && make run: cd src && make run -# At present there's no method to delete the contents of the database -# from w/in the program, so add a way to do so outside. -refresh_db: - gaiac gaia/slam.ddl --db-name slam -g -o /tmp/slam +drun: + cd src && make drun refresh: clean all clean: - rm -Rf build + cd gaia && make clean + cd src && make clean diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 98d5f27..723004a 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -60,6 +60,7 @@ ruleset map_ruleset : serial_group() // the graph optimization check in a serial group. on_insert(edges) { +printf("1 enter on_insert(edges)\n"); // New edge created. See if it's time to run new graph optimization. if (optimization_required()) { @@ -69,26 +70,31 @@ ruleset map_ruleset : serial_group() // done here, so do it here. build_area_map(/destination, /area_map, /observed_area); } +printf("1 leave on_insert(edges)\n"); } // Destination changed. Update working map and start moving toward it. on_change(destination) { +printf("2 enter on_change(destination)\n"); build_area_map(/destination, /area_map, /observed_area); move_toward_destination(/destination, /area_map); +printf("2 leave on_change(destination)\n"); } // For each vertex, update working map using new sensor data. on_insert(vertices) { +printf("3 enter on_insert(vertices)\n"); // If working map will go beyond border of area map, rebuild // area map. - if (need_to_extend_map()) + if (need_to_extend_map(vertices.position->positions, /observed_area)) { build_area_map(/destination, /area_map, /observed_area); } +printf("3 leave on_insert(vertices)\n"); } } @@ -123,25 +129,31 @@ ruleset runtime_ruleset on_insert(vertices) { +printf("4 enter on_insert(vertices)\n"); // Determine if it's time to change destinations. if (reassess_destination()) { // If destination changes, start a new graph. } +printf("4 leave on_insert(vertices)\n"); } on_insert(collision_event) { +printf("5 enter on_insert(collision_event)\n"); // A collision was detected, or the range sensor detected something // closer than it's supposed to be. Initiate a stop, which will // in turn create a new observation. gaia_log::app().info("Collision detected. Stopping bot"); full_stop(); +printf("5 leave on_insert(collision_event)\n"); } on_insert(graphs) { +printf("6 enter on_insert(graphs)\n"); /ego.current_graph_id = graphs.id; +printf("6 leave on_insert(graphs)\n"); } } diff --git a/slam_sim/include/sensor_data.hpp b/slam_sim/include/sensor_data.hpp index a519c4a..dc59dda 100644 --- a/slam_sim/include/sensor_data.hpp +++ b/slam_sim/include/sensor_data.hpp @@ -31,7 +31,7 @@ constexpr double RANGE_SENSOR_MAX_METERS = 4.0; struct sensor_data_t { - uint32_t num_radials; +// uint32_t num_radials; // Range on each radial. Range is -1 if there's no data. std::vector range_meters; diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 544a73b..7096e96 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -46,7 +46,8 @@ void optimize_graph(gaia::slam::graphs_t&); // Checks to see if known world has extended beyong map boundaries so much // that it's time to rebuild the area map. -bool need_to_extend_map(); +bool need_to_extend_map(gaia::slam::positions_t& pos, + gaia::slam::observed_area_t& bounds); // Generates a low-res map off the known world and generates coarse path // info to the destination. diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index ec558b0..8ed0673 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -107,12 +107,14 @@ void load_world_map(const char* world_map) // making up the 2D world map. void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { +printf("Calculating range data\n"); data.range_meters.clear(); data.bearing_degs.clear(); - float step_degs = c_range_sensor_sweep_degs / (data.num_radials - 1); + float step_degs = c_range_sensor_sweep_degs / (c_num_range_radials - 1); // Get range on each radial, and store both distance and radial degs. - for (uint32_t n=0; n r(n_pix, 0), g(n_pix, 0), b(n_pix, 0); + // Create image. for (uint32_t y=0; ydata; // New grid. Nodes need initialization. clear(); } else { assert(m_grid_size.rows == am.num_rows()); assert(m_grid_size.cols == am.num_cols()); + m_grid = (map_node_t*) blob->data; + // Existing grid. Should be initialized already. } - m_grid = (map_node_t*) blob->data; } diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 0fe0133..8c73035 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -32,6 +32,7 @@ using std::string; using gaia::slam::edges_t; using gaia::slam::ego_t; using gaia::slam::graphs_t; +using gaia::slam::positions_t; using gaia::slam::vertices_t; using gaia::slam::area_map_t; @@ -50,11 +51,26 @@ using gaia::slam::destination_writer; bool optimization_required() { static int32_t ctr = 0; - const int32_t MAP_INTERVAL = 10; - if ((++ctr % MAP_INTERVAL) == 0) + if (ctr < 4) { return true; } + else if (ctr < 8) + { + return (ctr & 1) == 0; + } + else if (ctr < 16) + { + return (ctr & 3) == 0; + } + else if (ctr < 32) + { + return (ctr & 7) == 0; + } + else + { + return (ctr & 15) == 0; + } return false; } @@ -104,9 +120,29 @@ void optimize_graph(graphs_t& graph) // Determines if sensor data extends beyond present area map and returns // 'true' if so. -bool need_to_extend_map() +bool need_to_extend_map(positions_t& pos, observed_area_t& bounds) { - assert(1 == 0); + float right_meters = ceilf(pos.x_meters() + c_range_sensor_max_meters); + float top_meters = ceilf(pos.y_meters() + c_range_sensor_max_meters); + float left_meters = floorf(pos.x_meters() - c_range_sensor_max_meters); + float bottom_meters = floorf(pos.y_meters() - c_range_sensor_max_meters); + if (right_meters > bounds.right_meters()) + { + return true; + } + if (left_meters < bounds.left_meters()) + { + return true; + } + if (top_meters > bounds.top_meters()) + { + return true; + } + if (bottom_meters < bounds.bottom_meters()) + { + return true; + } + // Else, sensor range doesn't exceed map boundaries. return false; } @@ -137,7 +173,7 @@ static void build_map(const graphs_t& g, const world_coordinate_t& bottom_left, void export_map_to_file() { - gaia::db::begin_transaction(); + //gaia::db::begin_transaction(); ego_t ego = *(ego_t::list().begin()); area_map_t& am = *(area_map_t::list().begin()); world_coordinate_t bottom_left = { @@ -147,7 +183,7 @@ void export_map_to_file() float width_meters = am.right_meters() - am.left_meters(); float height_meters = am.top_meters() - am.bottom_meters(); build_map(ego.current_graph(), bottom_left, width_meters, height_meters); - gaia::db::commit_transaction(); + //gaia::db::commit_transaction(); } @@ -175,7 +211,7 @@ void build_area_map(destination_t& dest, area_map_t& am, area_map_writer writer = am.writer(); writer.blob_id = area_map.get_blob_id(); writer.update_row(); -// export_area_map(); + export_map_to_file(); } diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 915616c..07ad206 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -119,10 +119,10 @@ printf("Creating vertex\n"); // Get most recent obesrvation. vertices_t prev_vert = ego.latest_observation().vertex(); - positions_t prev_pos = prev_vert.position(); float prev_x_meters, prev_y_meters, prev_heading_degs; - if (prev_pos) + if (prev_vert) { + positions_t prev_pos = prev_vert.position(); prev_x_meters = prev_pos.x_meters(); prev_y_meters = prev_pos.y_meters(); prev_heading_degs = prev_pos.heading_degs(); @@ -148,8 +148,10 @@ printf("Creating vertex\n"); ); // create range_data record + assert(data.bearing_degs.size() == c_num_range_radials); + assert(data.range_meters.size() == c_num_range_radials); gaia_id_t range_id = range_data_t::insert_row( - data.num_radials, // num_radials + c_num_range_radials, // num_radials data.bearing_degs, // bearing_degs data.range_meters // distance_meters ); From e7c71b3cf7c1b05ea8746dc4960eadcdd9c24490 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Thu, 28 Apr 2022 15:56:05 -0700 Subject: [PATCH 20/30] update unit tests --- slam_sim/.gitignore | 2 + slam_sim/gaia/slam.ruleset | 26 +++-- slam_sim/include/globals.hpp | 8 +- slam_sim/include/map_types.hpp | 70 ++++++++++++++ slam_sim/include/occupancy.hpp | 81 +--------------- slam_sim/include/slam_sim.hpp | 7 +- slam_sim/src/Makefile | 17 +++- slam_sim/src/analyze.cpp | 14 +-- slam_sim/src/globals.cpp | 30 ++++++ slam_sim/src/main.cpp | 24 ++--- slam_sim/src/rule_api.cpp | 118 ++++++----------------- slam_sim/src/support.cpp | 116 +++++++++++++++++++--- slam_sim/src/test_analyze.cpp | 107 ++++++++++++++++++++ slam_sim/src/utils/Makefile | 9 +- slam_sim/src/utils/line_segment.cpp | 32 +++--- slam_sim/src/utils/test_blob_cache.cpp | 70 ++++++++++++++ slam_sim/src/utils/test_line_segment.cpp | 70 ++++++++++++++ 17 files changed, 563 insertions(+), 238 deletions(-) create mode 100644 slam_sim/include/map_types.hpp create mode 100644 slam_sim/src/globals.cpp create mode 100644 slam_sim/src/test_analyze.cpp create mode 100644 slam_sim/src/utils/test_blob_cache.cpp create mode 100644 slam_sim/src/utils/test_line_segment.cpp diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index 8964162..08cd5a8 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -6,3 +6,5 @@ slam_sim *.o logs *.pgm +test_analyze +z* diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 723004a..19a1d03 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -20,6 +20,7 @@ #include #include +#include "globals.hpp" #include "slam_sim.hpp" /*********************************************************************** @@ -38,16 +39,16 @@ ruleset ***********************************************************************/ -using slam_sim::optimization_required; -using slam_sim::optimize_graph; - -using slam_sim::need_to_extend_map; using slam_sim::build_area_map; -//using slam_sim::build_working_map; - -using slam_sim::reassess_destination; -using slam_sim::move_toward_destination; using slam_sim::full_stop; +using slam_sim::move_toward_destination; +using slam_sim::need_to_extend_map; +using slam_sim::optimize_graph; +using slam_sim::optimization_required; +using slam_sim::reassess_destination; +using slam_sim::update_world_area; + +using slam_sim::g_running; // Wrap error correction and map creation in a serial group as these are @@ -78,8 +79,13 @@ printf("1 leave on_insert(edges)\n"); on_change(destination) { printf("2 enter on_change(destination)\n"); + // Destination's changed. Make sure the known world includes + // this point. + update_world_area(/ego); + // Build map, including paths, to destination. build_area_map(/destination, /area_map, /observed_area); - move_toward_destination(/destination, /area_map); + // Start movement toward destination. + g_running = true; printf("2 leave on_change(destination)\n"); } @@ -133,7 +139,7 @@ printf("4 enter on_insert(vertices)\n"); // Determine if it's time to change destinations. if (reassess_destination()) { - // If destination changes, start a new graph. + // TODO If destination changes, start a new graph. } printf("4 leave on_insert(vertices)\n"); } diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp index 1126dad..94461f0 100644 --- a/slam_sim/include/globals.hpp +++ b/slam_sim/include/globals.hpp @@ -10,11 +10,10 @@ #include -#include "blob_cache.hpp" -#include "occupancy.hpp" - namespace slam_sim { +class blob_cache_t; +struct world_coordinate_t; // Flag to indicate app should exit. Normally this is 0. When it's time // to quit it's set to 1. @@ -35,5 +34,8 @@ extern uint32_t g_next_destination; // Current simulation time. extern double g_now; +// Flag to indicate motion state (true=moving, false=halted). +extern bool g_running; + } // namespace slam_sim diff --git a/slam_sim/include/map_types.hpp b/slam_sim/include/map_types.hpp new file mode 100644 index 0000000..ae948c1 --- /dev/null +++ b/slam_sim/include/map_types.hpp @@ -0,0 +1,70 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// +#pragma once + +#include + +namespace slam_sim +{ + +//////////////////////////////////////////////////////////////////////// +// Supporting types. X,Y values are used in many different contexts. To +// help avoid mixing paradigms, different types are defined for the +// different contexts. + +// Physical location in world space. +// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). +struct world_coordinate_t +{ + float x_meters; + float y_meters; +}; + +// Bounding area of map in world space. +// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). +struct map_size_t +{ + float x_meters; + float y_meters; +}; + +// Dimensions of map (occupancy grid). +struct grid_size_t +{ + uint32_t rows; + uint32_t cols; +}; + +// Node location in grid. +// NOTE: this is in image coordinates (i.e., 0,0 is top left). +struct grid_coordinate_t +{ + uint32_t x; + uint32_t y; +}; + +// Distance from one grid node to another, in units of grid coordinates. +// NOTE: this is in image coordiantes (i.e., increasing x,y is down/right). +struct node_offset_t +{ + int32_t dx; + int32_t dy; +}; + +// Storage index of a node w/in the 2D grid. +struct grid_index_t +{ + // The index here is unsigned in order to help detect improper usage + // (e.g., index of -1). + uint32_t idx; +}; +constexpr uint32_t c_invalid_grid_idx = 0xffffffff; + + +} // namespace slam_sim + diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 604b56f..dec9513 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -31,65 +31,12 @@ The code has to manage the switch between the two. #include "gaia_slam.h" +#include "map_types.hpp" #include "sensor_data.hpp" namespace slam_sim { -//////////////////////////////////////////////////////////////////////// -// Supporting types. X,Y values are used in many different contexts. To -// help avoid mixing paradigms, different types are defined for the -// different contexts. - -// Physical location in world space. -// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). -struct world_coordinate_t -{ - float x_meters; - float y_meters; -}; - -// Bounding area of map in world space. -// NOTE: this is in world coordinates (i.e., increasing x,y is up/right). -struct map_size_t -{ - float x_meters; - float y_meters; -}; - -// Dimensions of map (occupancy grid). -struct grid_size_t -{ - uint32_t rows; - uint32_t cols; -}; - -// Node location in grid. -// NOTE: this is in image coordinates (i.e., 0,0 is top left). -struct grid_coordinate_t -{ - uint32_t x; - uint32_t y; -}; - -// Distance from one grid node to another, in units of grid coordinates. -// NOTE: this is in image coordiantes (i.e., increasing x,y is down/right). -struct node_offset_t -{ - int32_t dx; - int32_t dy; -}; - -// Storage index of a node w/in the 2D grid. -struct grid_index_t -{ - // The index here is unsigned in order to help detect improper usage - // (e.g., index of -1). - uint32_t idx; -}; -constexpr uint32_t c_invalid_grid_idx = 0xffffffff; - - //////////////////////////////////////////////////////////////////////// // The map is composed of several nodes arranged on a grid. @@ -270,31 +217,5 @@ class occupancy_grid_t const float path_weight); }; - -//class area_grid_t : public occupancy_grid_t -//{ -//public: -// // Constructors to be called by rules. -// // Loads existing map, if present (map will be blank if it's doesn't -// // exist yet). -// area_grid_t(gaia::slam::area_map_t&); -// // Purges existing map and rebuilds a new one, using observed area as -// // bounds. -// area_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); -//}; -// -// -//class working_grid_t : public occupancy_grid_t -//{ -//public: -// // Creates a grid that's not associated w/ database record. -// working_grid_t(); -// -// // Constructor to be called by rules. -// // Loads existinig working map. If this is to be embedded in larger -// // map, that must be done separately. -// working_grid_t(gaia::slam::working_map_t&); -//}; - } // namespace slam_sim diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 7096e96..812b884 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -60,9 +60,12 @@ void build_area_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, // returns 'false'. bool reassess_destination(); +// Updates the observed_area record to make sure it includes all observed +// areas plus the destination. +void update_world_area(gaia::slam::ego_t& ego); + // Instructs hardware layer to move toward destination. -void move_toward_destination(gaia::slam::destination_t&, - gaia::slam::area_map_t&); +void move_toward_destination(); // Instructs bot to stop. An observation will be taken here. void full_stop(); diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 43174b5..55310ac 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -9,8 +9,10 @@ endif TARGET = slam_sim +TEST_TARGETS = test_analyze -OBJS = main.o analyze.o occupancy.o rule_api.o path_map.o support.o +CORE_OBJS = analyze.o occupancy.o rule_api.o path_map.o support.o globals.o +OBJS = main.o $(CORE_OBJS) EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl @@ -18,11 +20,18 @@ all: $(OBJS) cd utils && make $(CXX) $(OBJS) $(EXT_OBJS) -o $(TARGET) $(LIBS) +test: $(TEST_TARGETS) + +test_analyze: test_analyze.cpp analyze.cpp $(CORE_OBJS) + $(CXX) $(CPPFLAGS) test_analyze.cpp -o test_analyze \ + $(CORE_OBJS) $(EXT_OBJS) $(LIBS) + + run: - ./$(TARGET) -m ../data/map.json -x -3.0 -y -3.0 + ./$(TARGET) -m ../data/map.json -x -3.0 -y 3.0 drun: - gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y -3.0 + gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y 3.0 %.o: %.cpp @@ -32,7 +41,7 @@ refresh: clean all clean: cd utils && make clean - rm -f *.o $(TARGET) + rm -f *.o $(TARGET) *.pgm rm -rf logs diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index 8ed0673..fc52c6b 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -25,8 +25,8 @@ #include "json.hpp" #include "globals.hpp" #include "line_segment.hpp" +#include "map_types.hpp" #include "sensor_data.hpp" -#include "slam_sim.hpp" namespace slam_sim { @@ -69,11 +69,6 @@ static void set_map(const char* map) dest.y_meters = point["y"]; g_destinations.push_back(dest); } - -for (world_coordinate_t w: g_destinations) -{ - printf("DESTINATION: %f,%f\n", w.x_meters, w.y_meters); -} } @@ -107,19 +102,19 @@ void load_world_map(const char* world_map) // making up the 2D world map. void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { -printf("Calculating range data\n"); +printf("Calculating range data from %f,%f\n", coord.x_meters, coord.y_meters); data.range_meters.clear(); data.bearing_degs.clear(); float step_degs = c_range_sensor_sweep_degs / (c_num_range_radials - 1); // Get range on each radial, and store both distance and radial degs. for (uint32_t n=0; n= 360.0 ? theta_degs - 360.0 : theta_degs; theta_degs = theta_degs < 0.0 ? theta_degs + 360.0 : theta_degs; +printf(" %d: %.1f\n", n, theta_degs); // Measure distance on this radial float min_meters = -1.0; int32_t line_num = -1; @@ -128,6 +123,7 @@ printf("Calculating range data\n"); line_segment_t& seg = g_world_lines[i]; float dist_meters = seg.intersect_range(coord.x_meters, coord.y_meters, theta_degs); +printf(" %.1f,%.1f -> %.1f,%.1f range %.2f\n", seg.m_x0, seg.m_y0, seg.m_x1, seg.m_y1, dist_meters); if (dist_meters > 0.0) { if ((min_meters < 0.0) || (dist_meters < min_meters)) @@ -141,7 +137,7 @@ printf("Calculating range data\n"); { min_meters = -1.0; } -//printf(" range %.1f at %.1f\n", min_meters, theta_degs); +printf(" range %.1f at %.1f\n", min_meters, theta_degs); data.range_meters.push_back(min_meters); data.bearing_degs.push_back(theta_degs); } diff --git a/slam_sim/src/globals.cpp b/slam_sim/src/globals.cpp new file mode 100644 index 0000000..0d5df0a --- /dev/null +++ b/slam_sim/src/globals.cpp @@ -0,0 +1,30 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +// Globals for this namespace + +#include + +#include "occupancy.hpp" + +namespace slam_sim +{ + +int32_t g_quit = 0; +world_coordinate_t g_position = { .x_meters = 0.0, .y_meters = 0.0 }; +float g_heading_degs = 0.0f; + +std::vector g_destinations; +uint32_t g_next_destination = 0; + +double g_now = 0.0; + +bool g_running = false; + +} // namespace slam_sim + diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 31e17e4..636c8d2 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -32,26 +32,28 @@ using std::this_thread::sleep_for; static constexpr uint32_t c_rule_wait_millis = 100; -// Globals for this namespace -int32_t g_quit = 0; -world_coordinate_t g_position = { .x_meters = 0.0, .y_meters = 0.0 }; -float g_heading_degs = 0.0f; - -std::vector g_destinations; -uint32_t g_next_destination = 0; - -double g_now = 0.0; - /** * Wait for simulation to complete. */ void main_loop() { +int32_t ctr = 0; // When the simulation completes it will set g_quit to 1. Then we can // exit. In the meantime, the simulation is being handled by rules. while (g_quit == 0) { - sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); + if (g_running) + { + move_toward_destination(); + if (ctr++ > 3) + { + g_quit = 1; + } + } + else + { + sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); + } } } diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 8c73035..5ee2e3f 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -50,28 +50,29 @@ using gaia::slam::destination_writer; // an optiimzation is required every X vertices. bool optimization_required() { - static int32_t ctr = 0; - if (ctr < 4) - { - return true; - } - else if (ctr < 8) - { - return (ctr & 1) == 0; - } - else if (ctr < 16) - { - return (ctr & 3) == 0; - } - else if (ctr < 32) - { - return (ctr & 7) == 0; - } - else - { - return (ctr & 15) == 0; - } - return false; + return true; +// static int32_t ctr = 0; +// if (ctr < 4) +// { +// return true; +// } +// else if (ctr < 8) +// { +// return (ctr & 1) == 0; +// } +// else if (ctr < 16) +// { +// return (ctr & 3) == 0; +// } +// else if (ctr < 32) +// { +// return (ctr & 7) == 0; +// } +// else +// { +// return (ctr & 15) == 0; +// } +// return false; } @@ -153,6 +154,7 @@ static void build_map(const graphs_t& g, const world_coordinate_t& bottom_left, static int32_t ctr = 0; occupancy_grid_t map(c_standard_map_node_width_meters, bottom_left, width_meters, height_meters); +printf("Building map between %.1f,%.1f and %.1f,%.1f\n", bottom_left.x_meters, bottom_left.y_meters, bottom_left.x_meters + width_meters, bottom_left.y_meters + height_meters); // Iterate through observations in this graph and build a map. for (const vertices_t& o: g.vertices()) { @@ -214,77 +216,14 @@ void build_area_map(destination_t& dest, area_map_t& am, export_map_to_file(); } - -void build_working_map(occupancy_grid_t& working_map, - destination_t& dest, area_map_t& am) -{ - // Apply observation data - for (vertices_t& v: vertices_t::list()) - { - working_map.apply_sensor_data(v); - } - // Find paths toward destination. - world_coordinate_t dest_coord = { - .x_meters = dest.x_meters(), - .y_meters = dest.y_meters() - }; - occupancy_grid_t area_grid(am); - area_grid.trace_routes(dest_coord, area_grid); -} - //////////////////////////////////////////////////////////////////////// - void full_stop() { - // TODO Take observation and stop moving. - // TODO Need way to decide when to move_toward_destination() again. -} - - -// Infrastructure has info on latest position so no need to query DB -// (infra is what sent that info to the DB). - - -void move_toward_destination(destination_t& dest, area_map_t& am) -{ - // Need position, area map, destination - // Generate working map. - world_coordinate_t bottom_left = { - .x_meters = floorf(-c_range_sensor_max_meters), - .y_meters = floorf(-c_range_sensor_max_meters) - }; - float right_meters = ceilf(c_range_sensor_max_meters); - float top_meters = ceilf(c_range_sensor_max_meters); - float width_meters = right_meters - bottom_left.x_meters; - float height_meters = top_meters - bottom_left.y_meters; - occupancy_grid_t working_map(c_working_map_node_width_meters, - bottom_left, width_meters, height_meters); - build_working_map(working_map, dest, am); - // Make several small steps. - for (uint32_t i=0; i= g_destinations.size()) { g_next_destination = 0; diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 07ad206..6f61b68 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -31,9 +31,11 @@ namespace slam_sim static int32_t s_next_vertex_id = 1; -// +using std::min; +using std::max; + using gaia::common::gaia_id_t; -// + using gaia::slam::area_map_t; using gaia::slam::edges_t; using gaia::slam::destination_t; @@ -55,13 +57,17 @@ using gaia::slam::observed_area_writer; // importantly, this means that the functions here are expected to // be called from within an active transaction -static void update_observed_area(ego_t& ego) +// Updates the observed_area record to make sure it includes all observed +// areas plus the destination. +void update_world_area(ego_t& ego) { observed_area_t area = ego.world(); + destination_t dest = ego.destination(); + bool change = false; // Left and right bounds. - float left_edge = area.left_meters(); - float right_edge = area.right_meters(); + float left_edge = min(area.left_meters(), dest.x_meters()); + float right_edge = max(area.right_meters(), dest.x_meters()); float max_range = c_range_sensor_max_meters; if (floor(g_position.x_meters - max_range) < left_edge) { @@ -74,8 +80,8 @@ static void update_observed_area(ego_t& ego) change = true; } // Top and bottom bounds. - float bottom_edge = area.bottom_meters(); - float top_edge = area.top_meters(); + float bottom_edge = min(area.bottom_meters(), dest.y_meters()); + float top_edge = max(area.top_meters(), dest.y_meters()); if (floor(g_position.y_meters - max_range) < bottom_edge) { bottom_edge = floor(g_position.y_meters - max_range); @@ -96,6 +102,7 @@ static void update_observed_area(ego_t& ego) oa_writer.top_meters = top_edge; oa_writer.update_row(); } +printf("World area: %.1f,%.1f to %.1f,%.1f\n", left_edge, bottom_edge, right_edge, top_edge); } @@ -174,7 +181,7 @@ printf("Creating vertex\n"); v.range_data().connect(range_id); v.motion().connect(movement_id); - update_observed_area(ego); + update_world_area(ego); // create edge if (prev_vert) @@ -197,6 +204,76 @@ printf("Creating vertex\n"); // Non-rule API // The functions here must manage their own transactions. +static void build_working_map(occupancy_grid_t& working_map, + destination_t& dest, area_map_t& am) +{ + // Apply observation data + for (vertices_t& v: vertices_t::list()) + { + working_map.apply_sensor_data(v); + } + // Find paths toward destination. + world_coordinate_t dest_coord = { + .x_meters = dest.x_meters(), + .y_meters = dest.y_meters() + }; + occupancy_grid_t area_grid(am); + area_grid.trace_routes(dest_coord, area_grid); +} + + +// Infrastructure has info on latest position so no need to query DB +// (infra is what sent that info to the DB). +void move_toward_destination() +{ + gaia::db::begin_transaction(); + destination_t& dest = *(destination_t::list().begin()); + area_map_t& am = *(area_map_t::list().begin()); + // + // Need position, area map, destination + // Generate working map. + world_coordinate_t bottom_left = { + .x_meters = floorf(-c_range_sensor_max_meters), + .y_meters = floorf(-c_range_sensor_max_meters) + }; + float right_meters = ceilf(c_range_sensor_max_meters); + float top_meters = ceilf(c_range_sensor_max_meters); + float width_meters = right_meters - bottom_left.x_meters; + float height_meters = top_meters - bottom_left.y_meters; + occupancy_grid_t working_map(c_working_map_node_width_meters, + bottom_left, width_meters, height_meters); + build_working_map(working_map, dest, am); + // Make several small steps. + for (uint32_t i=0; i 1); + // Area map and observed area start out with same dimensions. - float left = floorf(x_meters - (c_range_sensor_max_meters+1)); - float bottom = floorf(y_meters - (c_range_sensor_max_meters+1)); - float right = ceilf(x_meters + (c_range_sensor_max_meters+1)); - float top = ceilf(y_meters + (c_range_sensor_max_meters+1)); + // Shorthand. + float max_range = c_range_sensor_max_meters; + // Align calls laterally for visual inspection to help make sure variables + // are correct. + float left = floorf(min(x_meters, new_dest.x_meters) - (max_range+1)); + float bottom = floorf(min(y_meters, new_dest.y_meters) - (max_range+1)); + float right = ceilf(max(x_meters, new_dest.x_meters) + (max_range+1)); + float top = ceilf(max(y_meters, new_dest.y_meters) + (max_range+1)); gaia_id_t world_id = observed_area_t::insert_row( left, // left_meters @@ -239,11 +323,11 @@ void seed_database(float x_meters, float y_meters) ); gaia_id_t destination_id = destination_t::insert_row( - x_meters, // x_meters - y_meters, // y_meters - 0.0 // departure_time_sec + new_dest.x_meters, // x_meters + new_dest.y_meters, // y_meters + 0.0 // departure_time_sec ); -printf("initial destination %f,%f\n", x_meters, y_meters); +printf("initial destination %f,%f\n", new_dest.x_meters, new_dest.y_meters); //////////////////////////////////////////// // Relationships diff --git a/slam_sim/src/test_analyze.cpp b/slam_sim/src/test_analyze.cpp new file mode 100644 index 0000000..a84afe5 --- /dev/null +++ b/slam_sim/src/test_analyze.cpp @@ -0,0 +1,107 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include + +#include + +#include "json.hpp" +#include "line_segment.hpp" +#include "map_types.hpp" +#include "sensor_data.hpp" +#include "slam_sim.hpp" + +constexpr const char* WORLD_MAP_FILE = "../data/map.json"; + +using namespace slam_sim; +using namespace slam_sim; + +void print_sensor_data(sensor_data_t& data) +{ + for (uint32_t i=0; i 0.01) + { + fprintf(stderr, "Radial %d (%.1f degs) has range of %.3f, " + "expected %.3f\n", num, data.bearing_degs[num], + data.range_meters[num], expected); + errs++; + } + return errs; +} + + +int main(int argc, char** argv) +{ + (void) argc; + (void) argv; + uint32_t errs = 0; + + gaia::system::initialize(); + + //////////////////////////////////////////////////////////////////// + load_world_map(WORLD_MAP_FILE); + std::string response; + sensor_data_t data; + map_coord_t coord = { + .x_meters = -3.0, + .y_meters = 4.4, + .heading_degs = 45.0 + }; + calculate_range_data(coord, data); + errs += check_radial(0, 0.5, data); + errs += check_radial(22, 0.7, data); + errs += check_radial(44, 2.9, data); + + coord.heading_degs = 135.0; + calculate_range_data(coord, data); + errs += check_radial(22, -1.0, data); + + // table distances + // 207 degs. Strikes vertical edge of table near bottom. + // sqrt(1^2 + (1/tan(28))^2) + coord.heading_degs = 208.0; + calculate_range_data(coord, data); + errs += check_radial(22, 2.130, data); + + // 228.5 degs. Strikes horizontal edge of table near right edge. + // sqrt(0.9^2 + (0.9/tan(41.5))^2) + errs += check_radial(32, 1.358, data); + + // 245 degs. Strikes horizontal edge of table near left edge. + // sqrt(0.9^2 + (0.9*tan(25.2))^2) + calculate_range_data(coord, data); + errs += check_radial(40, 2.114, data); + + //////////////////////////////////////////////////////////////////// + if (errs > 0) + { + fprintf(stderr, "********************************\n"); + fprintf(stderr, "Encountered %d errors\n", errs); + } + else + { + fprintf(stderr, "All tests pass\n"); + } + + return static_cast(errs); +} diff --git a/slam_sim/src/utils/Makefile b/slam_sim/src/utils/Makefile index 61aaf3f..65d4d12 100644 --- a/slam_sim/src/utils/Makefile +++ b/slam_sim/src/utils/Makefile @@ -1,6 +1,6 @@ include ../../Makefile.inc -TEST_TARGETS = test_line_segment +TEST_TARGETS = test_line_segment test_blob_cache OBJS = line_segment.o blob_cache.o @@ -11,7 +11,10 @@ all: $(OBJS) test: $(TEST_TARGETS) test_line_segment: line_segment.cpp - $(CXX) $(CPPFLAGS) line_segment.cpp -o test_line_segment -DTEST_LINE_SEGMENT + $(CXX) $(CPPFLAGS) test_line_segment.cpp line_segment.cpp -o zline_segment + +test_blob_cache: blob_cache.cpp + $(CXX) $(CPPFLAGS) test_blob_cache.cpp blob_cache.cpp -o zblob_cache %.o: %.cpp @@ -20,6 +23,6 @@ test_line_segment: line_segment.cpp refresh: clean all clean: - rm -f *.o *.a $(TEST_TARGETS) + rm -f *.o *.a $(TEST_TARGETS) z* diff --git a/slam_sim/src/utils/line_segment.cpp b/slam_sim/src/utils/line_segment.cpp index 149fd6d..f926bc3 100644 --- a/slam_sim/src/utils/line_segment.cpp +++ b/slam_sim/src/utils/line_segment.cpp @@ -15,6 +15,10 @@ // radial. The nearest line segment on each radial is selected which // will correspond to the nearest wall. // +// Coordinate system is map-based, with positive Y being upward and +// positive X being rightward. It is NOT the coordinate system for +// image processing (which has positive Y being downward). +// //////////////////////////////////////////////////////////////////////// #include @@ -35,9 +39,7 @@ line_segment_t::line_segment_t(double x0, double y0, double x1, double y1) m_x1 = x1; m_y1 = y1; // derived values - // Y value is in screen coords (+Y is downward). Invert that for - // computations. - m_a = y0 - y1; + m_a = y1 - y0; m_b = x1 - x0; m_c = m_a*x0 + m_b*y0; m_len = sqrt(m_a*m_a + m_b*m_b); @@ -75,16 +77,16 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) // Segment 1 double x1 = m_x0; double x2 = m_x1; - double y1 = -m_y0; - double y2 = -m_y1; + double y1 = m_y0; + double y2 = m_y1; // Segment 2 double x3 = x; double x4 = x3 + 10.0 * sin(D2R * theta); - double y3 = -y; - double y4 = -y + 10.0 * cos(D2R * theta); + double y3 = y; + double y4 = y + 10.0 * cos(D2R * theta); //printf("This segment: %.1f,%.1f -> %.1f,%.1f\n", x1, y1, x2, y2); //printf(" -> line %.1f,%.1f at %.1f\n", x3, y3, theta); -//printf(" other: %.1f,%.1f -> %.4f,%.4f\n", x3, y3, x4, y4); +//printf(" becomes: %.1f,%.1f -> %.4f,%.4f\n", x3, y3, x4, y4); // Px = ((x1y2 - x2y1)(x3-x4) - (x1-x2)(x3y4-y3x4)) / denom; // Py = ((x1y2 - y1x2)(y3-y4) - (y1-y2)(x3y4-y3x4)) / denom; // denom = (x1-x2)(y3-y4) - (y1-y2)(x3-x4); @@ -105,7 +107,8 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) //printf(" intersect point %.4f,%.4f\n", px, py); // Intersection may or may not be in the direction of theta. // All this algorithm tells us is that the lines, infinitely - // expanded, meet at this point. + // expanded, meet at this point. Find out if intersection + // point is in the direction of theta. double inter_theta = unwrap_compass(R2D * atan2(px-x3, py-y3)); double delta_degs = inter_theta - theta; if (delta_degs < -180.0) @@ -116,12 +119,19 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) { delta_degs -= 360.0; } -//printf(" measured theta: %.1f delta: %.1f\n", inter_theta, delta_degs); +//printf(" measured theta: %.1f delta: %f\n", inter_theta, delta_degs); if (fabs(delta_degs) < 1.0) { - // intersection point is in direction of theta + // Intersection point is in direction of theta. Now see if + // it falls w/in the bounds of this line segment, or if the + // point of intersection is on an extension of this line. + // Distance from each end line segment to the intersection point: double dist0 = measure_distance(px-x1, py-y1); double dist1 = measure_distance(px-x2, py-y2); + // If longest distance is less than line segment length then + // the intersection point is w/in the segment. +//printf("dist 0: %f\n", dist0); +//printf("dist 1: %f\n", dist1); if ((dist0 <= m_len) && (dist1 <= m_len)) { dist = measure_distance(px-x3, py-y3); diff --git a/slam_sim/src/utils/test_blob_cache.cpp b/slam_sim/src/utils/test_blob_cache.cpp new file mode 100644 index 0000000..07c2fb2 --- /dev/null +++ b/slam_sim/src/utils/test_blob_cache.cpp @@ -0,0 +1,70 @@ +//////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////// + +#include + +#include + +#include "blob_cache.hpp" +#include "retail_assert.hpp" + +using namespace std; + +namespace slam_sim +{ + +void test_blob_cache(); + +void test_blob_cache() +{ + blob_cache_t cache; + // Create two blobs. + cache.create_or_reset_blob(1, 1, 0); + cache.create_or_reset_blob(2, 1, 1); + + // Read back the information of the blobs that we created. + blob_t* blob_ptr = cache.get_blob(1); + cout << "Blob 1 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; + + blob_ptr = cache.get_blob(2); + cout << "Blob 2 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; + + // Create a third blob. It should lead to the deletion of the first blob. + cache.create_or_reset_blob(3, 1, 2); + + blob_ptr = cache.get_blob(3); + cout << "Blob 3 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; + + // Also update this blob's state. + blob_ptr->state = blob_state_t::used; + cout << "Blob 3 state is now '" << (size_t)blob_ptr->state << "'" << endl; + + // Verify that the first blob has been deleted. + blob_ptr = cache.get_blob(1); + RETAIL_ASSERT( + !blob_ptr, "FAILED: Blob 1 still exists!"); + cout << "Blob 1 has been removed, as expected!" << endl; + + // Reset the third blob state. + cache.create_or_reset_blob(3, 1, 2); + + // Read the third blob and verify that its state has been reset. + blob_ptr = cache.get_blob(3); + RETAIL_ASSERT( + blob_ptr->state == blob_state_t::initialized, + "FAILED: Blob 3 state has not been reset!"); + cout << "Blob 3 has been reset, as expected!" << endl; +} + +} // namespace slam_sim + +int main(void) +{ + slam_sim::test_blob_cache(); +} + diff --git a/slam_sim/src/utils/test_line_segment.cpp b/slam_sim/src/utils/test_line_segment.cpp new file mode 100644 index 0000000..6821f06 --- /dev/null +++ b/slam_sim/src/utils/test_line_segment.cpp @@ -0,0 +1,70 @@ +//////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +//////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "line_segment.hpp" + +using namespace std; + +using slam_sim::line_segment_t; + +uint32_t check_distance(double len, double expected) +{ + if (fabs(len - expected) > 0.001) + { + fprintf(stderr, "Distance out of range. Got %.3f, expected %.3f\n", + len, expected); + return 1; + } + return 0; +} + +int main() +{ + uint32_t errs = 0; + + // horizontal line at y=10 (i.e., 10 units above the origin) + line_segment_t a(-10.0, 10.0, 10.0, 10.0); + errs += check_distance(a.intersect_range(0.0, 0.0, 0.0), 10.0); + errs += check_distance(a.intersect_range(0.0, 0.0, 30.0), 11.547); + errs += check_distance(a.intersect_range(0.0, 0.0, -30.0), 11.547); + errs += check_distance(a.intersect_range(0.0, 0.0, 60.0), -1.0); + errs += check_distance(a.intersect_range(0.0, 0.0, 150.0), -1.0); + errs += check_distance(a.intersect_range(0.0, 0.0, 270.0), -1.0); + + // diagonal line crossing from 0,10 to 10,0 (i.e., 1st quadrant) + line_segment_t b(0.0, 10.0, 10.0, 0.0); + errs += check_distance(b.intersect_range(0.0, 0.0, 0.1), 9.983); + errs += check_distance(b.intersect_range(0.0, 0.0, -0.01), -1.0); + errs += check_distance(b.intersect_range(0.0, 0.0, 359.99), -1.0); + errs += check_distance(b.intersect_range(0.0, 0.0, 45.0), 7.071); + errs += check_distance(b.intersect_range(0.0, 0.0, 225.0), -1.0); + + // opposite orientation horizontal line at y=10 + line_segment_t c(10.0, 10.0, -10.0, 10.0); + errs += check_distance(c.intersect_range(0.0, 0.0, 0.0), 10.0); + errs += check_distance(c.intersect_range(0.0, 0.0, 30.0), 11.547); + errs += check_distance(c.intersect_range(0.0, 0.0, -30.0), 11.547); + errs += check_distance(c.intersect_range(0.0, 0.0, 60.0), -1.0); + errs += check_distance(c.intersect_range(0.0, 0.0, 150.0), -1.0); + errs += check_distance(c.intersect_range(0.0, 0.0, 270.0), -1.0); + + if (errs > 0) + { + fprintf(stderr, "********************************\n"); + fprintf(stderr, "Encountered %d errors\n", errs); + } + else + { + fprintf(stderr, "All tests pass\n"); + } + + return static_cast(errs); +} From 35715fbe31f576b6d0fef67f7a2fb4d9bf507cba Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Thu, 28 Apr 2022 16:00:59 -0700 Subject: [PATCH 21/30] comment out debug statements --- slam_sim/src/analyze.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index fc52c6b..de557fe 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -102,7 +102,7 @@ void load_world_map(const char* world_map) // making up the 2D world map. void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { -printf("Calculating range data from %f,%f\n", coord.x_meters, coord.y_meters); +//printf("Calculating range data from %f,%f\n", coord.x_meters, coord.y_meters); data.range_meters.clear(); data.bearing_degs.clear(); float step_degs = c_range_sensor_sweep_degs / (c_num_range_radials - 1); @@ -114,7 +114,7 @@ printf("Calculating range data from %f,%f\n", coord.x_meters, coord.y_meters); + (float) n * step_degs; theta_degs = theta_degs >= 360.0 ? theta_degs - 360.0 : theta_degs; theta_degs = theta_degs < 0.0 ? theta_degs + 360.0 : theta_degs; -printf(" %d: %.1f\n", n, theta_degs); +//printf(" %d: %.1f\n", n, theta_degs); // Measure distance on this radial float min_meters = -1.0; int32_t line_num = -1; @@ -123,7 +123,7 @@ printf(" %d: %.1f\n", n, theta_degs); line_segment_t& seg = g_world_lines[i]; float dist_meters = seg.intersect_range(coord.x_meters, coord.y_meters, theta_degs); -printf(" %.1f,%.1f -> %.1f,%.1f range %.2f\n", seg.m_x0, seg.m_y0, seg.m_x1, seg.m_y1, dist_meters); +//printf(" %.1f,%.1f -> %.1f,%.1f range %.2f\n", seg.m_x0, seg.m_y0, seg.m_x1, seg.m_y1, dist_meters); if (dist_meters > 0.0) { if ((min_meters < 0.0) || (dist_meters < min_meters)) @@ -137,7 +137,7 @@ printf(" %.1f,%.1f -> %.1f,%.1f range %.2f\n", seg.m_x0, seg.m_y0, seg.m_x1 { min_meters = -1.0; } -printf(" range %.1f at %.1f\n", min_meters, theta_degs); +//printf(" range %.1f at %.1f\n", min_meters, theta_degs); data.range_meters.push_back(min_meters); data.bearing_degs.push_back(theta_degs); } From cbe1431bf0ce7b393fdbd0e09dda3de0a7b0beb6 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Fri, 29 Apr 2022 09:45:27 -0700 Subject: [PATCH 22/30] revised blob cache to avoid segfault, updated unit test --- slam_sim/.gitignore | 2 + slam_sim/include/blob_cache.hpp | 39 +++----- slam_sim/src/utils/Makefile | 6 +- slam_sim/src/utils/README_blob_cache.md | 55 ++++++++--- slam_sim/src/utils/blob_cache.cpp | 124 ++++++++++-------------- slam_sim/src/utils/test_blob_cache.cpp | 124 ++++++++++++++++-------- 6 files changed, 195 insertions(+), 155 deletions(-) diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index 08cd5a8..6c0c80a 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -7,4 +7,6 @@ slam_sim logs *.pgm test_analyze +test_blob_cache +test_line_segment z* diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp index 4c01516..f3964bf 100644 --- a/slam_sim/include/blob_cache.hpp +++ b/slam_sim/include/blob_cache.hpp @@ -33,41 +33,34 @@ namespace slam_sim constexpr uint32_t c_invalid_blob_id = 0; -enum class blob_state_t : uint8_t -{ - not_set = 0, - - initialized = 1, - used = 2, -}; -struct blob_t +class blob_t { public: blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob); ~blob_t(); - // Deallocate blob memory. - void clear(); + uint32_t id() { return m_id; } + size_t size() { return m_size; } + uint8_t* data() { return m_data; } - // Reset blob content. - void reset(uint32_t id, size_t size, uint32_t id_superseded_blob); + uint32_t supersedes() { return m_id_superseded_blob; } -public: - uint32_t id; +protected: + friend class blob_cache_t; - // Blob data information. - size_t size; - uint8_t* data; + uint32_t m_id; - // Blob state. - blob_state_t state; + // Blob data information. + size_t m_size; + uint8_t* m_data; // Blob that this blob supersedes. // It will be deleted when this blob itself is superseded. - uint32_t id_superseded_blob; + uint32_t m_id_superseded_blob; }; + class blob_cache_t { public: @@ -76,7 +69,7 @@ class blob_cache_t blob_cache_t& operator=(const blob_cache_t&) = delete; // Creates a blob with the specified ID and characteristics - blob_t* create_or_reset_blob(uint32_t id, size_t size, + blob_t* create_blob(uint32_t id, size_t size, uint32_t id_superseded_blob = c_invalid_blob_id); // Retrieves a blob given an id. @@ -89,8 +82,8 @@ class blob_cache_t std::shared_mutex m_lock; // Nested get_blob() call from w/in create_or_reset_blob(), where a - // lock has already been established. - blob_t* get_blob_locked(uint32_t id); + // mutex lock has already been established. + blob_t* get_blob_(uint32_t id); }; } // namespace slam_sim diff --git a/slam_sim/src/utils/Makefile b/slam_sim/src/utils/Makefile index 65d4d12..dcf127c 100644 --- a/slam_sim/src/utils/Makefile +++ b/slam_sim/src/utils/Makefile @@ -11,10 +11,10 @@ all: $(OBJS) test: $(TEST_TARGETS) test_line_segment: line_segment.cpp - $(CXX) $(CPPFLAGS) test_line_segment.cpp line_segment.cpp -o zline_segment + $(CXX) $(CPPFLAGS) test_line_segment.cpp line_segment.cpp -o test_line_segment test_blob_cache: blob_cache.cpp - $(CXX) $(CPPFLAGS) test_blob_cache.cpp blob_cache.cpp -o zblob_cache + $(CXX) $(CPPFLAGS) test_blob_cache.cpp blob_cache.cpp -o test_blob_cache %.o: %.cpp @@ -23,6 +23,6 @@ test_blob_cache: blob_cache.cpp refresh: clean all clean: - rm -f *.o *.a $(TEST_TARGETS) z* + rm -f *.o *.a $(TEST_TARGETS) diff --git a/slam_sim/src/utils/README_blob_cache.md b/slam_sim/src/utils/README_blob_cache.md index c88a08f..3f56a2b 100644 --- a/slam_sim/src/utils/README_blob_cache.md +++ b/slam_sim/src/utils/README_blob_cache.md @@ -1,25 +1,50 @@ # A mechanism for storing non-persistent memory blobs. -​ -Must support blob allocation during a transaction and be resistent to memory leaks when transactions are rolled back and retried. -Primary use case is creating a memory allocation that stores non-primary data that's used during other computations. Non-primary means that the data in the blob can be (re)generated anytime from content stored in the database. +Must support blob allocation during a transaction and be resistent +to memory leaks when transactions are rolled back and retried. + +Primary use case is creating a memory allocation that stores non-primary +data that's used during other computations. Non-primary means that the +data in the blob can be (re)generated anytime from content stored in the +database. + If a blob exists it can be used by the calling process. -If it doesn't exist then the calling process is responsible for initializing it. +If it doesn't exist then the calling process is responsible for +initializing it. + +One need for such a blob is for storing map data in SLAM, as navigation +requires a large blob that represents obstacles and routes to a destination. -One need for such a blob is for storing map data in SLAM, as navigation requires a large blob that represents obstacles and routes to a destination. +The database stores an ID for the blob, and sends that ID to the blob +factory, asking for the memory associated w/ that blob. If the blob doesn't +exist it is created. The factory needs to communicate to the calling process +whether the blob existed or if it was freshly created. +This is done by using get() to fetch the blob. If it exists a valid pointer +to it is returned. Otherwise NULL is returned, in which case the calling +processes needs to call create(). If create() is called and +the blob with a given ID already exists, then the existing blob is returned. +The calling function will not necessarily know if it's a new or existing blob, +although it's guaranteed that a new blob will have its data field initialized +to NULL (e.g., uint8\_t\* data = new uint8\_t\[size]]();) -The database stores an ID for the blob, and sends that ID to the blob factory, asking for the memory associated w/ that blob. -If the blob doesn't exist it is created. -The factory needs to communicate to the calling process whether the blob existed or if it was freshly created. -One way to do this is to have a flag in the blob that indicates if its data is valid/current. -The ID is managed by the calling process and that's controlled by the ID. -If blobs are created and destroyed, there must be a safe way to delete old ones, and this must operate in a world where a delete call may be issued many times (e.g., if called repeatedly during retried transactions) and should be tolerant of a delete being ignored (e.g., failed transaction that's not retried). -This is achieved by each blob having a reference to a blob that it supersedes, so if A is allocated and B is meant to supersede A (e.g., because of a resize), then a pointer to A is stored within B. -Even though A is superseded, it remains a valid memory allocation until B is superseded. +If blobs are created and destroyed, there must be a safe way to delete old +ones, and this must operate in a world where a delete call may be issued +many times (e.g., if called repeatedly during retried transactions) and +should be tolerant of a delete being ignored (e.g., failed transaction +that's not retried). + +This is achieved by each blob having a reference to a blob that it +supersedes, so if A is allocated and B is meant to supersede A (e.g., +because of a resize), then a pointer to A is stored within B. +Even though A is superseded, it remains a valid memory allocation until +B is superseded. + Only when B is superseded will A be deleted. -If B is allocated but the transaction is rolled back and not retried, then A will remain in use. -B will be allocated but won't be used until the calling process asks the blob factory for a blob w/ B's ID. + +If B is allocated but the transaction is rolled back and not retried, +then A will remain in use. B will be allocated but won't be used until +the calling process asks the blob factory for a blob w/ B's ID. As noted earlier, the calling process must manage the IDs for memory. A simple way to do this is to store the blob ID in a table. When creating a new blob during a transaction, increment the ID and ask the factory for a blob with this new ID. diff --git a/slam_sim/src/utils/blob_cache.cpp b/slam_sim/src/utils/blob_cache.cpp index 5aa8605..6ef4076 100644 --- a/slam_sim/src/utils/blob_cache.cpp +++ b/slam_sim/src/utils/blob_cache.cpp @@ -13,114 +13,90 @@ namespace slam_sim { blob_t::blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob) -{ - reset(id, size, id_superseded_blob); -} - -blob_t::~blob_t() -{ - clear(); -} - -void blob_t::clear() -{ - if (data != nullptr) - { - delete[] data; - data = nullptr; - } -} - -void blob_t::reset(uint32_t id, size_t size, uint32_t id_superseded_blob) { RETAIL_ASSERT( size > 0, "Attempted to allocate a blob of 0 size!"); - clear(); + m_id = id; + m_size = size; + m_id_superseded_blob = id_superseded_blob; + m_data = new uint8_t[size](); +} - this->id = id; - this->size = size; - this->id_superseded_blob = id_superseded_blob; - this->state = blob_state_t::initialized; +blob_t::~blob_t() +{ + RETAIL_ASSERT( + m_data != nullptr, + "Deleting blob NULL data element. Was it double deleted?"); - data = new uint8_t[size]; + delete[] m_data; + m_data = nullptr; } +//////////////////////////////////////////////////////////////////////// -blob_t* blob_cache_t::create_or_reset_blob(uint32_t id, size_t size, +blob_t* blob_cache_t::create_blob(uint32_t id, size_t size, uint32_t id_superseded_blob) { - RETAIL_ASSERT( - id != c_invalid_blob_id, - "Attempting to create a blob with an invalid id!"); - RETAIL_ASSERT( - size > 0, - "Attempting to create a blob with a 0 size!"); + if ((size == 0) || (id == c_invalid_blob_id)) + { + return nullptr; + } std::unique_lock unique_lock(m_lock); - // Lookup superseded blob. - blob_t* superseded_blob_ptr = nullptr; + // Lookup superseded blob. If it exists, and if it itself has a + // superseded blob, then delete the latter. if (id_superseded_blob != c_invalid_blob_id) { - superseded_blob_ptr = get_blob_locked(id_superseded_blob); - } - - // Check if a blob with this id already exists; - // if it exists, then reuse it; - // if not, then create a new one and insert it in our map. - bool is_new_blob = false; - blob_t* blob_ptr = get_blob_locked(id); - if (blob_ptr) - { - blob_ptr->reset(id, size, id_superseded_blob); - } - else - { - is_new_blob = true; - blob_ptr = new blob_t(id, size, id_superseded_blob); - m_blob_map.insert(std::pair(id, blob_ptr)); + blob_t* superseded_blob = nullptr; + superseded_blob = get_blob_(id_superseded_blob); + if (superseded_blob != nullptr) + { + uint32_t corpse_id = superseded_blob->supersedes(); + if (corpse_id != c_invalid_blob_id) { + // Delete corpse and remove all references to it. + blob_t* corpse = get_blob_(corpse_id); + delete corpse; + m_blob_map[corpse_id] = nullptr; + superseded_blob->m_id_superseded_blob = c_invalid_blob_id; + } + + } } - // Check if we have a previously superseded blob to delete. - // We only need to do this for new blobs, because old blobs have - // already gone through this. - if ((is_new_blob && superseded_blob_ptr) && - (superseded_blob_ptr->id_superseded_blob != c_invalid_blob_id)) + // Check if a blob with this id already exists. If so, reuse it. If not, + // create a new one and insert it into our map. + blob_t* blob = get_blob_(id); + if (blob == nullptr) { - auto iterator = - m_blob_map.find(superseded_blob_ptr->id_superseded_blob); - RETAIL_ASSERT( - iterator != m_blob_map.end(), - "Failed to find the previously superseded blob!"); - delete iterator->second; - m_blob_map.erase(iterator); + blob = new blob_t(id, size, id_superseded_blob); + m_blob_map.insert(std::pair(id, blob)); } - return blob_ptr; + return blob; } + blob_t* blob_cache_t::get_blob(uint32_t id) { - RETAIL_ASSERT( - id != c_invalid_blob_id, - "Attempting to retrieve a blob using an invalid id!"); - + if (id == c_invalid_blob_id) + { + return nullptr; + } std::shared_lock shared_lock(m_lock); - - auto iterator = m_blob_map.find(id); - return (iterator == m_blob_map.end()) ? nullptr : iterator->second; + return get_blob_(id); } -blob_t* blob_cache_t::get_blob_locked(uint32_t id) + +blob_t* blob_cache_t::get_blob_(uint32_t id) { RETAIL_ASSERT( id != c_invalid_blob_id, "Attempting to retrieve a blob using an invalid id!"); - auto iterator = m_blob_map.find(id); - return (iterator == m_blob_map.end()) ? nullptr : iterator->second; + return (m_blob_map.count(id) > 0) ? m_blob_map[id] : nullptr; } } // namespace slam_sim diff --git a/slam_sim/src/utils/test_blob_cache.cpp b/slam_sim/src/utils/test_blob_cache.cpp index 07c2fb2..247bd9c 100644 --- a/slam_sim/src/utils/test_blob_cache.cpp +++ b/slam_sim/src/utils/test_blob_cache.cpp @@ -18,53 +18,97 @@ using namespace std; namespace slam_sim { -void test_blob_cache(); - -void test_blob_cache() +static int32_t test_blob_cache() { + uint32_t errs = 0; blob_cache_t cache; - // Create two blobs. - cache.create_or_reset_blob(1, 1, 0); - cache.create_or_reset_blob(2, 1, 1); - - // Read back the information of the blobs that we created. - blob_t* blob_ptr = cache.get_blob(1); - cout << "Blob 1 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - blob_ptr = cache.get_blob(2); - cout << "Blob 2 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - // Create a third blob. It should lead to the deletion of the first blob. - cache.create_or_reset_blob(3, 1, 2); - - blob_ptr = cache.get_blob(3); - cout << "Blob 3 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - // Also update this blob's state. - blob_ptr->state = blob_state_t::used; - cout << "Blob 3 state is now '" << (size_t)blob_ptr->state << "'" << endl; - - // Verify that the first blob has been deleted. - blob_ptr = cache.get_blob(1); - RETAIL_ASSERT( - !blob_ptr, "FAILED: Blob 1 still exists!"); - cout << "Blob 1 has been removed, as expected!" << endl; - - // Reset the third blob state. - cache.create_or_reset_blob(3, 1, 2); - - // Read the third blob and verify that its state has been reset. - blob_ptr = cache.get_blob(3); - RETAIL_ASSERT( - blob_ptr->state == blob_state_t::initialized, - "FAILED: Blob 3 state has not been reset!"); - cout << "Blob 3 has been reset, as expected!" << endl; + // Create blob. + blob_t* blob1 = cache.create_blob(1, 16, 0); + if (blob1 == nullptr) + { + fprintf(stderr, "Failed to create blob 1\n"); + errs++; + } + if (cache.get_blob(1) == nullptr) + { + fprintf(stderr, "Failed to fetch blob 1\n"); + errs++; + } + + // Create blob 2, superseding 1. + blob_t* blob2 = cache.create_blob(2, 16, 1); + if (blob2 == nullptr) + { + fprintf(stderr, "Failed to create blob 2\n"); + errs++; + } + if (cache.get_blob(2) == nullptr) + { + fprintf(stderr, "Failed to fetch blob 2\n"); + errs++; + } + + // Make sure 1's still around + if (cache.get_blob(1) == nullptr) + { + fprintf(stderr, "Blob 1 delete prematurely\n"); + errs++; + } + + // Create blob 3, superseding 2, which should eliminate 1 + blob_t* blob3 = cache.create_blob(3, 16, 2); + if (blob3 == nullptr) + { + fprintf(stderr, "Failed to create blob 3\n"); + errs++; + } + if (cache.get_blob(3) == nullptr) + { + fprintf(stderr, "Failed to fetch blob 3\n"); + errs++; + } + // Make sure 2's still around + if (cache.get_blob(2) == nullptr) + { + fprintf(stderr, "Blob 2 delete prematurely\n"); + errs++; + } + // Make sure 1 is gone + if (cache.get_blob(1) != nullptr) + { + fprintf(stderr, "Blob 1 was not deleted\n"); + errs++; + } + + // Try to create invalid blob. + blob_t* blob0 = cache.create_blob(0, 16, 0); + if (blob0 != nullptr) + { + fprintf(stderr, "Succeeded in creating invalid blob\n"); + errs++; + } + + //////////////////////////////////////////////////////////////////// + return errs; } } // namespace slam_sim + int main(void) { - slam_sim::test_blob_cache(); + uint32_t errs = 0; + errs += slam_sim::test_blob_cache(); + //////////////////////////////////////////////////////////////////// + if (errs > 0) + { + fprintf(stderr, "********************************\n"); + fprintf(stderr, "Encountered %d errors\n", errs); + } + else + { + fprintf(stderr, "All tests pass\n"); + } + return static_cast(errs); } From da1a7488c2db86598c7a0eb4b8ab3b65a76747cf Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Fri, 29 Apr 2022 22:34:08 -0700 Subject: [PATCH 23/30] basic maze following behaviors working --- slam_sim/.gitignore | 1 + slam_sim/gaia/slam.ddl | 4 - slam_sim/include/constants.hpp | 8 +- slam_sim/include/occupancy.hpp | 12 +- slam_sim/src/Makefile | 5 +- slam_sim/src/main.cpp | 3 +- slam_sim/src/occupancy.cpp | 189 ++++++++++++++++-------- slam_sim/src/path_map.cpp | 63 ++++++-- slam_sim/src/rule_api.cpp | 52 ++++--- slam_sim/src/support.cpp | 55 +++---- slam_sim/src/utils/README_blob_cache.md | 2 +- 11 files changed, 249 insertions(+), 145 deletions(-) diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index 6c0c80a..ba0ce4d 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -6,6 +6,7 @@ slam_sim *.o logs *.pgm +*.pnm test_analyze test_blob_cache test_line_segment diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index d7cccec..e6e1b04 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -31,7 +31,6 @@ database slam; -- Single record tables: -- ego Stores state of the bot. -- area_map Map of the known world. ----- working_map Version of world map with recent sensor overlay. -- destination Location that bot is moving to. -- observed_area Bounds of observed world. -- @@ -84,7 +83,6 @@ table ego -- Map oriented. map references area_map --- working_map references working_map ) @@ -110,7 +108,6 @@ table area_map ( -- Reference to in-memory blob that stores 2D map. -- Blob ID changes on each map update. - -- Area map must be seeded with a different blob_id than working map. blob_id uint32, -- Bounding polygon @@ -124,7 +121,6 @@ table area_map num_cols uint32, ego references ego --- working_map references working_map ) diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 1a150a9..c039232 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -17,8 +17,8 @@ constexpr float c_range_sensor_max_meters = 4.0f; constexpr float c_range_sensor_sweep_degs = 90.0f; constexpr int32_t c_num_range_radials = 45; -constexpr float c_area_map_node_width_meters = 0.25f; -constexpr float c_working_map_node_width_meters = 0.05f; +constexpr float c_area_map_node_width_meters = 0.1f; +//constexpr float c_working_map_node_width_meters = 0.05f; constexpr float c_standard_map_node_width_meters = 0.05f; // Path finding. @@ -34,9 +34,9 @@ constexpr float c_path_penalty_per_observation = 0.05f; constexpr uint32_t c_num_ancestors_for_direction = 5; // Distance between each step. Keyframes are taken after several steps. -constexpr float c_step_meters = 2.0f * c_working_map_node_width_meters; +constexpr float c_step_meters = c_area_map_node_width_meters; -constexpr uint32_t c_num_steps_between_keyframes = 5; +constexpr uint32_t c_num_steps_between_keyframes = 4; // How close to destination bot has to declare it's reached it. constexpr float c_destination_radius_meters = 0.5f; diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index dec9513..f574a49 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -56,8 +56,13 @@ struct map_node_flags_t void clear(); }; -#define PATH_NODE_FLAG_PROCESSED 0x01 -#define PATH_NODE_FLAG_IMPASSABLE 0x02 +// Indicates whether or not a node has been processed, if it's impassable +// (i.e., a boundary was observed there) or if it's adjacent an +// impassable square) +#define PATH_NODE_FLAG_PROCESSED 0x01 +#define PATH_NODE_FLAG_IMPASSABLE 0x02 +#define PATH_NODE_FLAG_ADJ_IMPASSABLE 0x04 +#define PATH_NODE_FLAG_CLOSE_IMPASSABLE 0x08 // Characteristics of a node in the map, including distance from this node // to a/the destination. @@ -97,6 +102,9 @@ struct map_node_t // The grid (map) itself. class occupancy_grid_t { +public: + void count_bounds(); + public: // Creates new uncached map. occupancy_grid_t(float node_width_meters, world_coordinate_t bottom_left, diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 55310ac..c4b8135 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -41,7 +41,8 @@ refresh: clean all clean: cd utils && make clean - rm -f *.o $(TARGET) *.pgm + rm -f *.o $(TARGET) *.pnm rm -rf logs - +clean_images: + rm -f *.pnm diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 636c8d2..19deb4a 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -45,7 +45,8 @@ int32_t ctr = 0; if (g_running) { move_toward_destination(); - if (ctr++ > 3) + export_map_to_file(); + if (ctr++ > 30) { g_quit = 1; } diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index c2516e6..e5329e7 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -85,6 +85,76 @@ occupancy_grid_t::occupancy_grid_t(float node_width_meters, } +// Loads existing map. +occupancy_grid_t::occupancy_grid_t(area_map_t& am) +{ + world_coordinate_t bottom_left = { + .x_meters = am.left_meters(), + .y_meters = am.bottom_meters() + }; + init(c_area_map_node_width_meters, bottom_left, + am.right_meters() - am.left_meters(), + am.top_meters() - am.bottom_meters()); + // Recover memory from blob cache. + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + m_blob_id = am.blob_id();; + // Fetch existing blob. + blob_t* blob = g_area_blobs.get_blob(m_blob_id); + if (blob == NULL) + { + // Blob doesn't exist yet. This means that the process was + // started up against an existing database. This shouldn't + // happen, but allow it. Create it. + gaia_log::app().warn("Area map with ID {} did not have cached " + "blob. Recreating one.", m_blob_id); + size_t sz = num_nodes * sizeof *m_grid; + blob = g_area_blobs.create_blob(m_blob_id, sz); + m_grid = (map_node_t*) blob->data(); + // New grid. Nodes need initialization. + clear(); + } else { + assert(m_grid_size.rows == am.num_rows()); + assert(m_grid_size.cols == am.num_cols()); + m_grid = (map_node_t*) blob->data(); + // Existing grid. Should be initialized already. + } +printf("PULLING EXISTING MAP %d 0x%08lx\n", m_blob_id, (uint64_t) m_grid); +count_bounds(); +} + + +// Overwrites existing cached area map. +occupancy_grid_t::occupancy_grid_t(area_map_t& am, observed_area_t& area) +{ + world_coordinate_t bottom_left = { + .x_meters = am.left_meters(), + .y_meters = am.bottom_meters() + }; + init(c_area_map_node_width_meters, bottom_left, + area.right_meters() - area.left_meters(), + area.top_meters() - area.bottom_meters()); + // Get rid of old content and create a new empty blob. + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + uint32_t old_blob_id = am.blob_id(); + size_t sz = num_nodes * sizeof *m_grid; + m_blob_id = old_blob_id + 1; + blob_t* blob = g_area_blobs.create_blob(m_blob_id, sz, old_blob_id); + m_grid = (map_node_t*) blob->data(); + clear(); + // Store changes in DB. + area_map_writer writer = am.writer(); + writer.blob_id = m_blob_id; + writer.left_meters = area.left_meters(); + writer.right_meters = area.right_meters(); + writer.top_meters = area.top_meters(); + writer.bottom_meters = area.bottom_meters(); + writer.num_rows = m_grid_size.rows; + writer.num_cols = m_grid_size.cols; + writer.update_row(); +printf("BUILDING NEW MAP %d 0x%08lx\n", m_blob_id, (uint64_t) m_grid); +} + + occupancy_grid_t::~occupancy_grid_t() { if (m_blob_id < 0) @@ -125,6 +195,7 @@ void map_node_flags_t::clear() void occupancy_grid_t::clear() { +printf("OCCUPANCY GRID CLEAR\n"); for (uint32_t y=0; y r(n_pix, 0), g(n_pix, 0), b(n_pix, 0); +printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); +//printf("-----------------------------------------\n"); // Create image. for (uint32_t y=0; y 0.0) { float boundary = node.boundary / node.observed; @@ -343,73 +455,22 @@ void occupancy_grid_t::export_as_pnm(string file_name) } } -//////////////////////////////////////////////////////////////////////// -// area map constructors - -// Loads existing map. -occupancy_grid_t::occupancy_grid_t(area_map_t& am) +void occupancy_grid_t::count_bounds() { - world_coordinate_t bottom_left = { - .x_meters = am.left_meters(), - .y_meters = am.bottom_meters() - }; - init(c_area_map_node_width_meters, bottom_left, - am.right_meters() - am.left_meters(), - am.top_meters() - am.bottom_meters()); - // Recover memory from blob cache. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_blob_id = am.blob_id();; - blob_t* blob = NULL; - // Fetch existing blob. - g_area_blobs.get_blob(m_blob_id); - if (blob == NULL) + uint32_t cnt = 0; + for (uint32_t y=0; ydata; - // New grid. Nodes need initialization. - clear(); - } else { - assert(m_grid_size.rows == am.num_rows()); - assert(m_grid_size.cols == am.num_cols()); - m_grid = (map_node_t*) blob->data; - // Existing grid. Should be initialized already. + for (uint32_t x=0; x 0.0f) + { +//printf("%d,%d (%dx%d=%d) bounds %f\n", x, y, m_grid_size.cols, m_grid_size.rows, idx, m_grid[idx].boundary); + cnt++; + } + } } -} - - -// Overwrites existing cached area map. -occupancy_grid_t::occupancy_grid_t(area_map_t& am, observed_area_t& area) -{ - world_coordinate_t bottom_left = { - .x_meters = am.left_meters(), - .y_meters = am.bottom_meters() - }; - init(c_area_map_node_width_meters, bottom_left, - area.right_meters() - area.left_meters(), - area.top_meters() - area.bottom_meters()); - // Get rid of old content and create a new empty blob. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - uint32_t old_blob_id = am.blob_id(); - size_t sz = num_nodes * sizeof *m_grid; - m_blob_id = old_blob_id + 1; - blob_t* blob = g_area_blobs.create_or_reset_blob(m_blob_id, sz, - old_blob_id); - m_grid = (map_node_t*) blob->data; - clear(); - // Store changes in DB. - area_map_writer writer = am.writer(); - writer.blob_id = m_blob_id; - writer.left_meters = area.left_meters(); - writer.right_meters = area.right_meters(); - writer.top_meters = area.top_meters(); - writer.bottom_meters = area.bottom_meters(); - writer.update_row(); + printf("Boundary count: %d\n", cnt); } } // namespace slam_sim diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index f1ce5c1..1483ac4 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -166,21 +166,34 @@ void occupancy_grid_t::add_node_to_stack( // Point is in the world. Check it. grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; map_node_t& child_node = m_grid[new_idx.idx]; +//printf("Adding child %d,%d bounds: %f\n", new_x, new_y, child_node.boundary); if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { +//printf(" child %d,%d impassable\n", new_x, new_y); return; } - // Determine weight for traversing this node. Have lower weight to nodes - // infrequently observed/traversed to provide bias to take different - // routes. - float weight = 1.0f; - weight += child_node.observed * c_path_penalty_per_observation; + // Determine added weight for traversing this node. Have lower weight + // to nodes infrequently observed/traversed to provide bias to take + // different routes. + float weight = child_node.observed * c_path_penalty_per_observation; + // We don't need to add boundary consideration here because the above + // conditional checking impassable filtered out grid squares where + // a boundary existed. Add a sanity check to make sure. + assert(child_node.boundary == 0.0); + if (child_node.flags.state | PATH_NODE_FLAG_ADJ_IMPASSABLE) + { + weight += 50.0; + } + if (child_node.flags.state | PATH_NODE_FLAG_CLOSE_IMPASSABLE) + { + weight += 5.0; + } // Path tracing algorithm is deterministic and can follow vert or // horiz path too easily. Add some jitter to allow pulling in // from more accurate direction. double jitter = 0.0; drand48_r(&path_rand_, &jitter); - jitter = 0.1 * (jitter - 0.5); + jitter = 0.1 * jitter; float new_cost = root_node.path_cost + weight + traversal_cost + jitter; if (child_node.flags.state & PATH_NODE_FLAG_PROCESSED) { // This node is already-processed. See if this path might provide @@ -193,11 +206,13 @@ void occupancy_grid_t::add_node_to_stack( // New weight is lower -- allow node to be added to stack again to // propagate updated weight to neighbors. } +//printf("child %d,%d %f\n", new_x, new_y, child_node.boundary); // Add point to stack for future consideration. child_node.parent_idx = root_idx; child_node.path_cost = new_cost; - child_node.traversal_cost = weight; + child_node.traversal_cost = traversal_cost + weight; child_node.flags.state |= PATH_NODE_FLAG_PROCESSED; +//printf("%d,%d cost %f boundary %f\n", new_x, new_y, new_cost, child_node.boundary); m_queue.push(new_idx); } @@ -326,6 +341,7 @@ void occupancy_grid_t::compute_path_costs() // stepping between adjacent nodes) out by several nodes, and measuing // the bearing to that node. If the path turns sharply (e.g., around // an obstacle) then the path is only traced to the turning point. +//printf("---------------------Checking direction\n"); for (uint32_t y=0; y %d,%d is %.1f degs\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, theta_degs); root.direction_degs = theta_degs; } } @@ -415,7 +436,13 @@ world_coordinate_t occupancy_grid_t::get_node_location( void occupancy_grid_t::trace_routes(world_coordinate_t& destination, occupancy_grid_t& parent_map) { - clear(); + // clear path costs + for (uint32_t y=0; y %.2f,%.2f @ %.1f\n", g_position.x_meters, g_position.y_meters, g_heading_degs); } // Position moved. Wait a bit before proceeding, to account for // at least a little bit of travel time. diff --git a/slam_sim/src/utils/README_blob_cache.md b/slam_sim/src/utils/README_blob_cache.md index 3f56a2b..bd28458 100644 --- a/slam_sim/src/utils/README_blob_cache.md +++ b/slam_sim/src/utils/README_blob_cache.md @@ -53,7 +53,7 @@ incremented, so if it's retried it will request a blob with the same ID again. That request can simultaneously have the former ID be superseded. E.g.,: ``` - blob_t* blob = blob_factory_t.create_blob( + blob_t* blob = cache.create_blob( ID+1, // new blob ID size_in_bytes, // size of memory to be allocated ID // superceded blob From ffb8db4603cff76f0209ffd8079b5b18ac1bedfa Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Fri, 29 Apr 2022 23:32:20 -0700 Subject: [PATCH 24/30] Testing on mazes --- slam_sim/data/maze.json | 47 ++++++++++++++++++++++++++++++++++ slam_sim/data/maze2.json | 30 ++++++++++++++++++++++ slam_sim/include/constants.hpp | 11 ++++---- slam_sim/include/occupancy.hpp | 1 + slam_sim/src/Makefile | 7 +++-- slam_sim/src/main.cpp | 2 +- slam_sim/src/occupancy.cpp | 37 +++++++++++++++++++------- slam_sim/src/path_map.cpp | 16 +++++++----- slam_sim/src/rule_api.cpp | 7 ----- 9 files changed, 127 insertions(+), 31 deletions(-) create mode 100644 slam_sim/data/maze.json create mode 100644 slam_sim/data/maze2.json diff --git a/slam_sim/data/maze.json b/slam_sim/data/maze.json new file mode 100644 index 0000000..1fc828f --- /dev/null +++ b/slam_sim/data/maze.json @@ -0,0 +1,47 @@ +{ + "description": "Simple maze", + "width": 18.0, + "height": 10.0, + "world": [ + { + "name": "Walls", + "vertices": [ + { "x": -8.9, "y": 4.9 }, + { "x": -6.1, "y": 4.9 }, + { "x": -6.1, "y": -0.1 }, + { "x": -2.9, "y": -0.1 }, + { "x": -2.9, "y": 0.1 }, + { "x": -5.9, "y": 0.1 }, + { "x": -5.9, "y": 4.9 }, + { "x": 2.9, "y": 4.9 }, + { "x": 2.9, "y": 0.1 }, + { "x": -0.1, "y": 0.1 }, + { "x": -0.1, "y": -0.1 }, + { "x": 3.1, "y": -0.1 }, + { "x": 3.1, "y": 4.9 }, + { "x": 5.9, "y": 4.9 }, + { "x": 5.9, "y": -2.6 }, + { "x": 6.1, "y": -2.6 }, + { "x": 6.1, "y": 4.9 }, + { "x": 8.9, "y": 4.9 }, + { "x": 8.9, "y": -4.9 }, + { "x": 0.1, "y": -4.9 }, + { "x": 0.1, "y": 4.9 } + ] + }, + { + "name": "Island", + "vertices": [ + { "x": -3.1, "y": 2.6 }, + { "x": 0.1, "y": 2.6 }, + { "x": 0.1, "y": 2.4 }, + { "x": -3.1, "y": 2.4 }, + { "x": -3.1, "y": 2.6 } + ] + } + ], + "destinations": [ + { "x": 7.5, "y": 4.0 }, + { "x": -4.5, "y": 4.0 } + ] +} diff --git a/slam_sim/data/maze2.json b/slam_sim/data/maze2.json new file mode 100644 index 0000000..f26fc6d --- /dev/null +++ b/slam_sim/data/maze2.json @@ -0,0 +1,30 @@ +{ + "description": "Simple maze", + "width": 10.0, + "height": 8.0, + "world": [ + { + "name": "Walls", + "vertices": [ + { "x": -4.9, "y": 3.9 }, + { "x": -2.1, "y": 3.9 }, + { "x": -2.1, "y": -1.1 }, + { "x": -1.9, "y": -1.1 }, + { "x": -1.9, "y": 3.9 }, + { "x": 1.9, "y": 3.9 }, + { "x": 1.9, "y": -1.1 }, + { "x": 2.1, "y": -1.1 }, + + { "x": 2.1, "y": 3.9 }, + { "x": 4.9, "y": 3.9 }, + { "x": 4.9, "y": -3.9 }, + { "x": -4.9, "y": -3.9 }, + { "x": -4.9, "y": 3.9 } + ] + } + ], + "destinations": [ + { "x": 3.5, "y": 3.0 }, + { "x": 0.0, "y": 3.0 } + ] +} diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index c039232..c0fdc5a 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -8,17 +8,16 @@ namespace slam_sim constexpr float c_rad_to_deg = (float) (180.0 / M_PI); constexpr float c_deg_to_rad = (float) (M_PI / 180.0); -// Width (height) of grids in occupancy grid. -constexpr float c_map_node_width_meters = 0.1f; - // Max distance range sensors can get range data. constexpr float c_range_sensor_max_meters = 4.0f; // Width of range sensor sweep in front of bot. constexpr float c_range_sensor_sweep_degs = 90.0f; constexpr int32_t c_num_range_radials = 45; +// Width (height) of grids in occupancy grid. +// Map for navigation constexpr float c_area_map_node_width_meters = 0.1f; -//constexpr float c_working_map_node_width_meters = 0.05f; +// Map for output constexpr float c_standard_map_node_width_meters = 0.05f; // Path finding. @@ -34,9 +33,9 @@ constexpr float c_path_penalty_per_observation = 0.05f; constexpr uint32_t c_num_ancestors_for_direction = 5; // Distance between each step. Keyframes are taken after several steps. -constexpr float c_step_meters = c_area_map_node_width_meters; +constexpr float c_step_meters = 0.75 * c_area_map_node_width_meters; -constexpr uint32_t c_num_steps_between_keyframes = 4; +constexpr uint32_t c_num_steps_between_keyframes = 2; // How close to destination bot has to declare it's reached it. constexpr float c_destination_radius_meters = 0.5f; diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index f574a49..59fce29 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -63,6 +63,7 @@ struct map_node_flags_t #define PATH_NODE_FLAG_IMPASSABLE 0x02 #define PATH_NODE_FLAG_ADJ_IMPASSABLE 0x04 #define PATH_NODE_FLAG_CLOSE_IMPASSABLE 0x08 +#define PATH_NODE_FLAG_NEAR_IMPASSABLE 0x10 // Characteristics of a node in the map, including distance from this node // to a/the destination. diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index c4b8135..af2ddd5 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -28,10 +28,13 @@ test_analyze: test_analyze.cpp analyze.cpp $(CORE_OBJS) run: - ./$(TARGET) -m ../data/map.json -x -3.0 -y 3.0 + ./$(TARGET) -m ../data/maze2.json -x -3.5 -y 1.5 + #./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 + #./$(TARGET) -m ../data/map.json -x -3.0 -y 3.0 drun: - gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y 3.0 + gdb --args ./slam_sim -m ../data/maze.json -x -4.5 -y 1.25 + #gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y 3.0 %.o: %.cpp diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 19deb4a..430f98e 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -46,7 +46,7 @@ int32_t ctr = 0; { move_toward_destination(); export_map_to_file(); - if (ctr++ > 30) + if (ctr++ > 120) { g_quit = 1; } diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index e5329e7..27bbee0 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -22,6 +22,7 @@ #include "blob_cache.hpp" #include "constants.hpp" +#include "globals.hpp" #include "line_segment.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" @@ -351,18 +352,25 @@ void occupancy_grid_t::apply_flags() if (node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { // First 8 deltas are in first ring around node. Those - // are ADJ_IMPASSABLE. Remainder are CLOSE_IMPASSABLE. - const int32_t x_delta[24] = { + // are ADJ_IMPASSABLE. Second ring are CLOSE_IMPASSABLE. + // Third ring are NEAR_IMPASSABLE. + const int32_t x_delta[48] = { -1, 0, 1, -1, 1, -1, 0, 1, -2, -1, 0, 1, 2, -2, 2, -2, - 2, -2, 2, -2, -1, 0, 1, 2 + 2, -2, 2, -2, -1, 0, 1, 2, + -3, -2, -1, 0, 1, 2, 3, + -3, 3, -3, 3, -3, 3, -3, 3, -3, 3, + -3, -2, -1, 0, 1, 2, 3 }; - const int32_t y_delta[24] = { + const int32_t y_delta[48] = { -1, -1, -1, 0, 0, 1, 1, 1, -2, -2, -2, -2, -2, -1, -1, 0, - 0, 1, 1, 2, 2, 2, 2, 2 + 0, 1, 1, 2, 2, 2, 2, 2, + -3, -3, -3, -3, -3, -3, -3, + -2, -2, -1, -1, 0, 0, 1, 1, 2, 2, + 3, -3, 3, 3, 3, 3, 3 }; - for (uint32_t i=0; i<24; i++) + for (uint32_t i=0; i<48; i++) { uint32_t nbr_x = (uint32_t) ((int32_t) node.pos.x + x_delta[i]); @@ -376,11 +384,16 @@ void occupancy_grid_t::apply_flags() m_grid[nbr_idx].flags.state |= PATH_NODE_FLAG_ADJ_IMPASSABLE; } - else + else if (i < 24) { m_grid[nbr_idx].flags.state |= PATH_NODE_FLAG_CLOSE_IMPASSABLE; } + else + { + m_grid[nbr_idx].flags.state |= + PATH_NODE_FLAG_NEAR_IMPASSABLE; + } } } } @@ -419,14 +432,13 @@ void occupancy_grid_t::export_as_pnm(string file_name) printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); //printf("-----------------------------------------\n"); - // Create image. + // Draw position and boundaries. for (uint32_t y=0; y 0.0) { float boundary = node.boundary / node.observed; @@ -436,6 +448,13 @@ printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); g[idx] = node.occupied > 0.0 ? 255 : 0; } } + // Indicate destination. + world_coordinate_t destination = g_destinations[g_next_destination]; + const map_node_t& dest = + get_node(destination.x_meters, destination.y_meters); + uint32_t dest_idx = dest.pos.x + dest.pos.y * m_grid_size.cols; + b[dest_idx] = 255; + g[dest_idx] = 128; // Export image. try diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 1483ac4..3cba344 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -182,9 +182,13 @@ void occupancy_grid_t::add_node_to_stack( assert(child_node.boundary == 0.0); if (child_node.flags.state | PATH_NODE_FLAG_ADJ_IMPASSABLE) { - weight += 50.0; + weight += 100.0; } - if (child_node.flags.state | PATH_NODE_FLAG_CLOSE_IMPASSABLE) + else if (child_node.flags.state | PATH_NODE_FLAG_CLOSE_IMPASSABLE) + { + weight += 10.0; + } + else if (child_node.flags.state | PATH_NODE_FLAG_NEAR_IMPASSABLE) { weight += 5.0; } @@ -499,16 +503,16 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination) m_grid[idx].path_cost = 0.0f; } } -printf("TRACE ROUTES grid at 0x%08lx (%d)\n", (uint64_t) m_grid, m_blob_id); +//printf("TRACE ROUTES grid at 0x%08lx (%d)\n", (uint64_t) m_grid, m_blob_id); //count_bounds(); grid_index_t idx = get_node_index(destination.x_meters, destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); -printf("Added anchor\n"); +//printf("Added anchor\n"); //count_bounds(); compute_path_costs(); -printf("Computed costs\n"); -count_bounds(); +//printf("Computed costs\n"); +//count_bounds(); } } // namespace slam_sim diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 16398fc..6870277 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -152,13 +152,6 @@ static void build_map(area_map_t& am) { static int32_t ctr = 0; occupancy_grid_t map(am); -// // Iterate through observations in this graph and build a map. -// for (const vertices_t& o: g.vertices()) -// { -// gaia_log::app().info("Applying sensor data from vertex {}", -// o.id()); -// map.apply_sensor_data(o); -// } // There are C++ ways to format a number to a string (e.g., // fmt::format or std::stringstream, but the C approach is // nice in its simplicity. From 11b97818381549845c2f102bd350630108f1563c Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Sat, 30 Apr 2022 16:01:50 -0700 Subject: [PATCH 25/30] Commit before moving out of rules --- slam_sim/gaia/slam.ruleset | 24 ++++----- slam_sim/include/constants.hpp | 8 +-- slam_sim/include/occupancy.hpp | 8 +-- slam_sim/include/slam_sim.hpp | 8 ++- slam_sim/src/Makefile | 2 +- slam_sim/src/main.cpp | 7 +-- slam_sim/src/occupancy.cpp | 46 ++++++++-------- slam_sim/src/path_map.cpp | 77 +++++++++++++------------- slam_sim/src/rule_api.cpp | 98 +++++++++++++++++++++++----------- slam_sim/src/support.cpp | 11 ++-- slam_sim/src/txn.cpp | 46 ++++++++++++++++ 11 files changed, 217 insertions(+), 118 deletions(-) create mode 100644 slam_sim/src/txn.cpp diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index 19a1d03..c929e9e 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -90,18 +90,18 @@ printf("2 leave on_change(destination)\n"); } - // For each vertex, update working map using new sensor data. - on_insert(vertices) - { -printf("3 enter on_insert(vertices)\n"); - // If working map will go beyond border of area map, rebuild - // area map. - if (need_to_extend_map(vertices.position->positions, /observed_area)) - { - build_area_map(/destination, /area_map, /observed_area); - } -printf("3 leave on_insert(vertices)\n"); - } +// // For each vertex, update working map using new sensor data. +// on_insert(vertices) +// { +//printf("3 enter on_insert(vertices)\n"); +// // If working map will go beyond border of area map, rebuild +// // area map. +// if (need_to_extend_map(vertices.position->positions, /observed_area)) +// { +// build_area_map(/destination, /area_map, /observed_area); +// } +//printf("3 leave on_insert(vertices)\n"); +// } } diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index c0fdc5a..036e4b9 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -16,9 +16,9 @@ constexpr int32_t c_num_range_radials = 45; // Width (height) of grids in occupancy grid. // Map for navigation -constexpr float c_area_map_node_width_meters = 0.1f; +constexpr float c_area_map_node_width_meters = 0.25f; // Map for output -constexpr float c_standard_map_node_width_meters = 0.05f; +constexpr float c_export_map_node_width_meters = 0.05f; // Path finding. // Bias factor for seeking out new areas to explore. This is the increase @@ -33,9 +33,9 @@ constexpr float c_path_penalty_per_observation = 0.05f; constexpr uint32_t c_num_ancestors_for_direction = 5; // Distance between each step. Keyframes are taken after several steps. -constexpr float c_step_meters = 0.75 * c_area_map_node_width_meters; +constexpr float c_step_meters = 0.5 * c_area_map_node_width_meters; -constexpr uint32_t c_num_steps_between_keyframes = 2; +constexpr uint32_t c_num_steps_between_keyframes = 1; // How close to destination bot has to declare it's reached it. constexpr float c_destination_radius_meters = 0.5f; diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 59fce29..d1ca3a9 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -103,9 +103,9 @@ struct map_node_t // The grid (map) itself. class occupancy_grid_t { -public: - void count_bounds(); - +//public: +// void count_bounds(); +// public: // Creates new uncached map. occupancy_grid_t(float node_width_meters, world_coordinate_t bottom_left, @@ -129,6 +129,8 @@ class occupancy_grid_t map_node_flags_t& get_node_flags(float x_meters, float y_meters); map_node_flags_t& get_node_flags(grid_index_t idx); +map_node_t* get_node_ptr(grid_index_t idx) { return &m_grid[idx.idx]; } + grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); // Returns the center of the grid node at the specified coordinates. diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 812b884..01d3cee 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -85,9 +85,15 @@ void generate_sensor_data(float pos_x_meters, float pos_y_meters, sensor_data_t& data); // Generates and exports a map to disk. Filename will be based on ego's -// timestamp. +// timestamp. Map is built from the most recent active area_map blob. It's +// what the bot is using for navigation. void export_map_to_file(); +// Generates and exports a map to disk. Filename will be based on ego's +// timestamp. Map is generated from scratch using content from the +// active graph. This is higher resolution than the navigatin map. +void build_export_map(); + // External request to move to a specific location. void request_destination(float x_meters, float y_meters); diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index af2ddd5..a734e0a 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -11,7 +11,7 @@ endif TARGET = slam_sim TEST_TARGETS = test_analyze -CORE_OBJS = analyze.o occupancy.o rule_api.o path_map.o support.o globals.o +CORE_OBJS = analyze.o occupancy.o rule_api.o path_map.o support.o globals.o txn.o OBJS = main.o $(CORE_OBJS) EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 430f98e..83f84af 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -29,7 +29,7 @@ namespace slam_sim using std::this_thread::sleep_for; -static constexpr uint32_t c_rule_wait_millis = 100; +static constexpr uint32_t c_rule_wait_millis = 200; /** @@ -45,8 +45,9 @@ int32_t ctr = 0; if (g_running) { move_toward_destination(); - export_map_to_file(); - if (ctr++ > 120) +// export_map_to_file(); + build_export_map(); + if (ctr++ > 40) { g_quit = 1; } diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 27bbee0..3c097fd 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -26,6 +26,7 @@ #include "line_segment.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" +#include "txn.hpp" namespace slam_sim { @@ -119,8 +120,6 @@ occupancy_grid_t::occupancy_grid_t(area_map_t& am) m_grid = (map_node_t*) blob->data(); // Existing grid. Should be initialized already. } -printf("PULLING EXISTING MAP %d 0x%08lx\n", m_blob_id, (uint64_t) m_grid); -count_bounds(); } @@ -429,7 +428,7 @@ void occupancy_grid_t::export_as_pnm(string file_name) { uint32_t n_pix = m_grid_size.cols * m_grid_size.rows; std::vector r(n_pix, 0), g(n_pix, 0), b(n_pix, 0); -printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); +//printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); //printf("-----------------------------------------\n"); // Draw position and boundaries. @@ -449,10 +448,15 @@ printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); } } // Indicate destination. - world_coordinate_t destination = g_destinations[g_next_destination]; + txn_t txn; + txn.begin(); + uint32_t which_dest = (g_next_destination + g_destinations.size() - 1) + % g_destinations.size(); + world_coordinate_t destination = g_destinations[which_dest]; const map_node_t& dest = get_node(destination.x_meters, destination.y_meters); uint32_t dest_idx = dest.pos.x + dest.pos.y * m_grid_size.cols; + txn.commit(); b[dest_idx] = 255; g[dest_idx] = 128; @@ -474,23 +478,23 @@ printf("Exporting 0x%08lx %d\n", (uint64_t) m_grid, m_blob_id); } } -void occupancy_grid_t::count_bounds() -{ - uint32_t cnt = 0; - for (uint32_t y=0; y 0.0f) - { -//printf("%d,%d (%dx%d=%d) bounds %f\n", x, y, m_grid_size.cols, m_grid_size.rows, idx, m_grid[idx].boundary); - cnt++; - } - } - } - printf("Boundary count: %d\n", cnt); -} +//void occupancy_grid_t::count_bounds() +//{ +// uint32_t cnt = 0; +// for (uint32_t y=0; y 0.0f) +// { +////printf("%d,%d (%dx%d=%d) bounds %f\n", x, y, m_grid_size.cols, m_grid_size.rows, idx, m_grid[idx].boundary); +// cnt++; +// } +// } +// } +// printf("Boundary count: %d\n", cnt); +//} } // namespace slam_sim diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 3cba344..b9ddaef 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -166,10 +166,10 @@ void occupancy_grid_t::add_node_to_stack( // Point is in the world. Check it. grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; map_node_t& child_node = m_grid[new_idx.idx]; -//printf("Adding child %d,%d bounds: %f\n", new_x, new_y, child_node.boundary); +printf("Adding child %d,%d bounds: %f\n", new_x, new_y, child_node.boundary); if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { -//printf(" child %d,%d impassable\n", new_x, new_y); +printf(" child %d,%d impassable\n", new_x, new_y); return; } // Determine added weight for traversing this node. Have lower weight @@ -180,15 +180,15 @@ void occupancy_grid_t::add_node_to_stack( // conditional checking impassable filtered out grid squares where // a boundary existed. Add a sanity check to make sure. assert(child_node.boundary == 0.0); - if (child_node.flags.state | PATH_NODE_FLAG_ADJ_IMPASSABLE) + if (child_node.flags.state & PATH_NODE_FLAG_ADJ_IMPASSABLE) { weight += 100.0; } - else if (child_node.flags.state | PATH_NODE_FLAG_CLOSE_IMPASSABLE) + else if (child_node.flags.state & PATH_NODE_FLAG_CLOSE_IMPASSABLE) { weight += 10.0; } - else if (child_node.flags.state | PATH_NODE_FLAG_NEAR_IMPASSABLE) + else if (child_node.flags.state & PATH_NODE_FLAG_NEAR_IMPASSABLE) { weight += 5.0; } @@ -210,13 +210,13 @@ void occupancy_grid_t::add_node_to_stack( // New weight is lower -- allow node to be added to stack again to // propagate updated weight to neighbors. } -//printf("child %d,%d %f\n", new_x, new_y, child_node.boundary); +printf("child %d,%d flags %02x boundary %f weight %f\n", new_x, new_y, child_node.flags.state, child_node.boundary, weight); // Add point to stack for future consideration. child_node.parent_idx = root_idx; child_node.path_cost = new_cost; child_node.traversal_cost = traversal_cost + weight; child_node.flags.state |= PATH_NODE_FLAG_PROCESSED; -//printf("%d,%d cost %f boundary %f\n", new_x, new_y, new_cost, child_node.boundary); +printf("%d,%d cost %f\n", new_x, new_y, new_cost); m_queue.push(new_idx); } @@ -345,7 +345,7 @@ void occupancy_grid_t::compute_path_costs() // stepping between adjacent nodes) out by several nodes, and measuing // the bearing to that node. If the path turns sharply (e.g., around // an obstacle) then the path is only traced to the turning point. -//printf("---------------------Checking direction\n"); +printf("---------------------Checking direction\n"); for (uint32_t y=0; y %d,%d is %.1f degs\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, theta_degs); root.direction_degs = theta_degs; +printf("%d,%d -> %d,%d is %.1f degs 0x%08lx\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, root.direction_degs, (uint64_t) &m_grid[idx]); } } } diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/rule_api.cpp index 6870277..7c9ed8d 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/rule_api.cpp @@ -23,6 +23,7 @@ #include "globals.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" +#include "txn.hpp" namespace slam_sim { @@ -83,26 +84,29 @@ bool optimization_required() // and just print error to the log. void optimize_graph(graphs_t& graph) { - // Map optimization logic goes here. Here are some ways to iterate - // through the data. - gaia_log::app().info("Stub function to optimize graph {}", graph.id()); - // By edges: - for (edges_t& e: edges_t::list()) + // Map optimization logic goes here. + //gaia_log::app().info("Stub function to optimize graph {}", graph.id()); + if (1 == 0) { - gaia_log::app().info("Edge connects vertices {} and {}", - e.src().id(), e.dest().id()); - } - // By vertices: - for (vertices_t& o: vertices_t::list()) - { - gaia_log::app().info("Obervation {} connects to:", o.id()); - for (edges_t& e: o.in_edges()) + // Here are some ways to itnerate through the data. + // By edges: + for (edges_t& e: edges_t::list()) { - gaia_log::app().info(" (in) obervation {}", e.src_id()); + gaia_log::app().info("Edge connects vertices {} and {}", + e.src().id(), e.dest().id()); } - for (edges_t& e: o.out_edges()) + // By vertices: + for (vertices_t& o: vertices_t::list()) { - gaia_log::app().info(" (out) obervation {}", e.dest_id()); + gaia_log::app().info("Obervation {} connects to:", o.id()); + for (edges_t& e: o.in_edges()) + { + gaia_log::app().info(" (in) obervation {}", e.src_id()); + } + for (edges_t& e: o.out_edges()) + { + gaia_log::app().info(" (out) obervation {}", e.dest_id()); + } } } // Create error correction record. This serves to store error correction @@ -165,22 +169,48 @@ static void build_map(area_map_t& am) void export_map_to_file() { - bool existing_transaction = false; - if (gaia::db::is_transaction_open()) - { - existing_transaction = true; - } - if (!existing_transaction) - { - gaia::db::begin_transaction(); - } + txn_t txn; + txn.begin(); ego_t ego = *(ego_t::list().begin()); area_map_t& am = *(area_map_t::list().begin()); build_map(am); - if (!existing_transaction) + txn.commit(); +} + + +// Build an map for export. This is typically a higher resolution than +// the navigation map. +void build_export_map() +{ + txn_t txn; + txn.begin(); + ego_t& ego = *(ego_t::list().begin()); + observed_area_t region = ego.world(); + const world_coordinate_t bottom_left = { + .x_meters = region.left_meters(), + .y_meters = region.bottom_meters(), + }; + float width_meters = region.right_meters() - region.left_meters(); + float height_meters = region.top_meters() - region.bottom_meters(); + // Each time we build an area map + occupancy_grid_t map(c_export_map_node_width_meters, + bottom_left, width_meters, height_meters); + for (graphs_t& g: graphs_t::list()) { - gaia::db::commit_transaction(); + for (vertices_t& v: g.vertices()) + { + //gaia_log::app().info("Pulling sensor data from {}:{}", + // g.id(), v.id()); + map.apply_sensor_data(v); + } } + txn.commit(); + + static int32_t ctr = 0; + char fname[256]; + sprintf(fname, "export_%03d.pnm", ctr++); + gaia_log::app().info("Building map {}", fname); + map.export_as_pnm(fname); } @@ -195,8 +225,8 @@ printf("Build area map\n"); //printf("Applying sensor data from graph %d\n", g.id()); for (vertices_t& v: g.vertices()) { - gaia_log::app().info("Pulling sensor data from {}:{}", - g.id(), v.id()); + //gaia_log::app().info("Pulling sensor data from {}:{}", + // g.id(), v.id()); //printf(" Sensor data from %d::%d\n", g.id(), v.id()); area_map.apply_sensor_data(v); } @@ -212,7 +242,7 @@ printf("Build area map\n"); area_map_writer writer = am.writer(); writer.blob_id = area_map.get_blob_id(); writer.update_row(); -area_map.count_bounds(); +//area_map.count_bounds(); } //////////////////////////////////////////////////////////////////////// @@ -228,6 +258,9 @@ void full_stop() bool reassess_destination() { + bool rc = false; + txn_t txn; + txn.begin(); // Determine if it's time to change destinations. // If w/in X meters of destination, select new one. It's OK // if destination is in unexplored area. @@ -251,9 +284,10 @@ printf("Next destination: %.1f,%.1f\n", new_dest.x_meters, new_dest.y_meters); w.y_meters = new_dest.y_meters; w.update_row(); g_now += 1.0; - return true; + rc = true; } - return false; + txn.commit(); + return rc; } } // namespace slam_sim diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 7d1ef5d..325965d 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -235,9 +235,13 @@ printf("MOVING\n"); for (uint32_t i=0; i %.2f,%.2f @ %.1f\n", g_position.x_meters, g_position.y_meters, g_heading_degs); } // Position moved. Wait a bit before proceeding, to account for // at least a little bit of travel time. diff --git a/slam_sim/src/txn.cpp b/slam_sim/src/txn.cpp new file mode 100644 index 0000000..147bbda --- /dev/null +++ b/slam_sim/src/txn.cpp @@ -0,0 +1,46 @@ +/////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +/////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////// +// +// Transaction wrapper. See txn.hpp for description. +// +/////////////////////////////////////////////////////////////////////// + +#include + +#include "txn.hpp" + +namespace slam_sim +{ + +txn_t::txn_t() +{ + m_manage_txn = !gaia::db::is_transaction_open(); +} + +bool txn_t::begin() +{ + if (m_manage_txn) + { + gaia::db::begin_transaction(); + } + return m_manage_txn; +} + +bool txn_t::commit() +{ + if (m_manage_txn) + { + gaia::db::commit_transaction(); + } + return m_manage_txn; +} + +} // namespace slam_sim + From c07ad0b2ab24b087569acd4d7cf72aaec4778a05 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Sun, 1 May 2022 10:38:55 -0700 Subject: [PATCH 26/30] successful nav --- slam_sim/data/maze2.json | 8 +- slam_sim/gaia/slam.ddl | 50 ++++++------ slam_sim/gaia/slam.ruleset | 130 +++++++++++++++--------------- slam_sim/include/constants.hpp | 5 +- slam_sim/include/globals.hpp | 11 ++- slam_sim/include/occupancy.hpp | 20 +++-- slam_sim/include/slam_sim.hpp | 17 ++-- slam_sim/src/globals.cpp | 3 + slam_sim/src/main.cpp | 17 ++-- slam_sim/src/occupancy.cpp | 143 ++++++++++----------------------- slam_sim/src/path_map.cpp | 67 +++++++-------- slam_sim/src/rule_api.cpp | 72 ++++++++--------- slam_sim/src/support.cpp | 58 ++++--------- 13 files changed, 252 insertions(+), 349 deletions(-) diff --git a/slam_sim/data/maze2.json b/slam_sim/data/maze2.json index f26fc6d..f881adf 100644 --- a/slam_sim/data/maze2.json +++ b/slam_sim/data/maze2.json @@ -8,12 +8,12 @@ "vertices": [ { "x": -4.9, "y": 3.9 }, { "x": -2.1, "y": 3.9 }, - { "x": -2.1, "y": -1.1 }, - { "x": -1.9, "y": -1.1 }, + { "x": -2.1, "y": -2.1 }, + { "x": -1.9, "y": -2.1 }, { "x": -1.9, "y": 3.9 }, { "x": 1.9, "y": 3.9 }, - { "x": 1.9, "y": -1.1 }, - { "x": 2.1, "y": -1.1 }, + { "x": 1.9, "y": -2.1 }, + { "x": 2.1, "y": -2.1 }, { "x": 2.1, "y": 3.9 }, { "x": 4.9, "y": 3.9 }, diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index e6e1b04..aa524e6 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -30,7 +30,7 @@ database slam; -- Table list -- Single record tables: -- ego Stores state of the bot. --- area_map Map of the known world. +---- area_map Map of the known world. -- destination Location that bot is moving to. -- observed_area Bounds of observed world. -- @@ -79,10 +79,10 @@ table ego -- Position oriented. destination references destination, - world references observed_area, + world references observed_area - -- Map oriented. - map references area_map +-- -- Map oriented. +-- map references area_map ) @@ -101,27 +101,27 @@ table observed_area ) --- Maps would ideally store map content as blobs but max blob size is --- presently too small. Instead, store pointer to current map and --- manage concurrent access manually. -table area_map -( - -- Reference to in-memory blob that stores 2D map. - -- Blob ID changes on each map update. - blob_id uint32, - - -- Bounding polygon - -- Uses world coordinates, with increasing X,Y being rightward/upward. - left_meters float, - right_meters float, - top_meters float, - bottom_meters float, - -- Grid size - num_rows uint32, - num_cols uint32, - - ego references ego -) +---- Maps would ideally store map content as blobs but max blob size is +---- presently too small. Instead, store pointer to current map and +---- manage concurrent access manually. +--table area_map +--( +--// -- Reference to in-memory blob that stores 2D map. +--// -- Blob ID changes on each map update. +--// blob_id uint32, +--// +-- -- Bounding polygon +-- -- Uses world coordinates, with increasing X,Y being rightward/upward. +-- left_meters float, +-- right_meters float, +-- top_meters float, +-- bottom_meters float, +-- -- Grid size +-- num_rows uint32, +-- num_cols uint32, +-- +-- ego references ego +--) table destination diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset index c929e9e..d55e442 100644 --- a/slam_sim/gaia/slam.ruleset +++ b/slam_sim/gaia/slam.ruleset @@ -39,16 +39,16 @@ ruleset ***********************************************************************/ -using slam_sim::build_area_map; +//using slam_sim::build_area_map; using slam_sim::full_stop; -using slam_sim::move_toward_destination; -using slam_sim::need_to_extend_map; -using slam_sim::optimize_graph; -using slam_sim::optimization_required; -using slam_sim::reassess_destination; -using slam_sim::update_world_area; +//using slam_sim::move_toward_destination; +//using slam_sim::need_to_extend_map; +//using slam_sim::optimize_graph; +//using slam_sim::optimization_required; +//using slam_sim::reassess_destination; +//using slam_sim::update_world_area; -using slam_sim::g_running; +//using slam_sim::g_running; // Wrap error correction and map creation in a serial group as these are @@ -56,40 +56,40 @@ using slam_sim::g_running; // experiencing a txn conflict, and avoid race conditions. ruleset map_ruleset : serial_group() { - // We don't want multiple edges to be created at similar points in time - // and both edges realize that it's time to optimize the map, so keep - // the graph optimization check in a serial group. - on_insert(edges) - { -printf("1 enter on_insert(edges)\n"); - // New edge created. See if it's time to run new graph optimization. - if (optimization_required()) - { - optimize_graph(edges.dest->vertices.graph->graphs); - // Whenever new error-correction data is available, regenerate - // maps. This can be triggered in a separate rule or simply - // done here, so do it here. - build_area_map(/destination, /area_map, /observed_area); - } -printf("1 leave on_insert(edges)\n"); - } - - - // Destination changed. Update working map and start moving toward it. - on_change(destination) - { -printf("2 enter on_change(destination)\n"); - // Destination's changed. Make sure the known world includes - // this point. - update_world_area(/ego); - // Build map, including paths, to destination. - build_area_map(/destination, /area_map, /observed_area); - // Start movement toward destination. - g_running = true; -printf("2 leave on_change(destination)\n"); - } - - +// // We don't want multiple edges to be created at similar points in time +// // and both edges realize that it's time to optimize the map, so keep +// // the graph optimization check in a serial group. +// on_insert(edges) +// { +//printf("1 enter on_insert(edges)\n"); +// // New edge created. See if it's time to run new graph optimization. +// if (optimization_required()) +// { +// optimize_graph(edges.dest->vertices.graph->graphs); +// // Whenever new error-correction data is available, regenerate +// // maps. This can be triggered in a separate rule or simply +// // done here, so do it here. +// build_area_map(/destination, /area_map, /observed_area); +// } +//printf("1 leave on_insert(edges)\n"); +// } +// +// +// // Destination changed. Update working map and start moving toward it. +// on_change(destination) +// { +//printf("2 enter on_change(destination)\n"); +// // Destination's changed. Make sure the known world includes +// // this point. +// update_world_area(/ego); +// // Build map, including paths, to destination. +// build_area_map(/destination, /area_map, /observed_area); +// // Start movement toward destination. +// g_running = true; +//printf("2 leave on_change(destination)\n"); +// } +// +// // // For each vertex, update working map using new sensor data. // on_insert(vertices) // { @@ -126,23 +126,23 @@ ruleset runtime_ruleset // made during processing will be invisible to the rule's txn. - on_insert(vertices) - { - // TODO Perform spatial query in area of observation and see if - // there are other vertices to build edges with (i.e., build - // loop closures) - } - - on_insert(vertices) - { -printf("4 enter on_insert(vertices)\n"); - // Determine if it's time to change destinations. - if (reassess_destination()) - { - // TODO If destination changes, start a new graph. - } -printf("4 leave on_insert(vertices)\n"); - } +// on_insert(vertices) +// { +// // TODO Perform spatial query in area of observation and see if +// // there are other vertices to build edges with (i.e., build +// // loop closures) +// } +// +// on_insert(vertices) +// { +//printf("4 enter on_insert(vertices)\n"); +// // Determine if it's time to change destinations. +// if (reassess_destination()) +// { +// // TODO If destination changes, start a new graph. +// } +//printf("4 leave on_insert(vertices)\n"); +// } on_insert(collision_event) { @@ -155,11 +155,11 @@ printf("5 enter on_insert(collision_event)\n"); printf("5 leave on_insert(collision_event)\n"); } - on_insert(graphs) - { -printf("6 enter on_insert(graphs)\n"); - /ego.current_graph_id = graphs.id; -printf("6 leave on_insert(graphs)\n"); - } +// on_insert(graphs) +// { +//printf("6 enter on_insert(graphs)\n"); +// /ego.current_graph_id = graphs.id; +//printf("6 leave on_insert(graphs)\n"); +// } } diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index 036e4b9..dfcb7dc 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include namespace slam_sim @@ -16,7 +17,7 @@ constexpr int32_t c_num_range_radials = 45; // Width (height) of grids in occupancy grid. // Map for navigation -constexpr float c_area_map_node_width_meters = 0.25f; +constexpr float c_navigation_map_node_width_meters = 0.25f; // Map for output constexpr float c_export_map_node_width_meters = 0.05f; @@ -33,7 +34,7 @@ constexpr float c_path_penalty_per_observation = 0.05f; constexpr uint32_t c_num_ancestors_for_direction = 5; // Distance between each step. Keyframes are taken after several steps. -constexpr float c_step_meters = 0.5 * c_area_map_node_width_meters; +constexpr float c_step_meters = 0.8 * c_navigation_map_node_width_meters; constexpr uint32_t c_num_steps_between_keyframes = 1; diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp index 94461f0..97df0ec 100644 --- a/slam_sim/include/globals.hpp +++ b/slam_sim/include/globals.hpp @@ -10,17 +10,22 @@ #include +#include "constants.hpp" +#include "occupancy.hpp" + namespace slam_sim { -class blob_cache_t; +//class blob_cache_t; struct world_coordinate_t; +extern occupancy_grid_t g_navigation_map; + // Flag to indicate app should exit. Normally this is 0. When it's time // to quit it's set to 1. extern int32_t g_quit; -// Blob caches for area maps and working maps. -extern blob_cache_t g_area_blobs; +//// Blob caches for area maps and working maps. +//extern blob_cache_t g_area_blobs; // Bot's position. This is known at the 'infrastructure' level (e.g., ROS) // which is where position data is fed to the DB from. This is the current diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index d1ca3a9..8440570 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -107,21 +107,17 @@ class occupancy_grid_t // void count_bounds(); // public: - // Creates new uncached map. + // Builds an empty map. This is for use when constructing global map. + // Map remains uninitialized. To make it usable, call reset(). + occupancy_grid_t(float node_width_meters); occupancy_grid_t(float node_width_meters, world_coordinate_t bottom_left, float width_meters, float height_meters); - // Constructors to be called by rules. - // Loads existing map, if present (map will be blank if it's doesn't - // exist yet). - occupancy_grid_t(gaia::slam::area_map_t&); - // Purges existing map and rebuilds a new one, using observed area as - // bounds. - occupancy_grid_t(gaia::slam::area_map_t&, gaia::slam::observed_area_t&); ~occupancy_grid_t(); - // Resets the map grid. - void clear(); + // Full map reset, including reallocating memory if necessary. + void reset(world_coordinate_t bottom_left, float width_meters, + float height_meters); // Returns a reference to tne map node at the specified location. map_node_t& get_node(float x_meters, float y_meters); @@ -171,7 +167,9 @@ map_node_t* get_node_ptr(grid_index_t idx) { return &m_grid[idx.idx]; } // Allocates own memory for m_grid, which is freed on destruction. // Grids created from DB blobs have memory for m_grid that is // managed externally. - void allocate_own_grid(); + void allocate_grid(bool raelloc=false); + // Initialize the map grid. + void initialize_grid(); void apply_radial(float radial_degs, float range_meters, float pos_x_meters, float pos_y_meters); diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index 01d3cee..aef3129 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -44,16 +44,13 @@ bool optimization_required(); void optimize_graph(gaia::slam::graphs_t&); -// Checks to see if known world has extended beyong map boundaries so much -// that it's time to rebuild the area map. -bool need_to_extend_map(gaia::slam::positions_t& pos, - gaia::slam::observed_area_t& bounds); +//// Checks to see if known world has extended beyong map boundaries so much +//// that it's time to rebuild the area map. +//bool need_to_extend_map(gaia::slam::positions_t& pos, +// gaia::slam::observed_area_t& bounds); -// Generates a low-res map off the known world and generates coarse path -// info to the destination. -// Stores output 'area_map' record. -void build_area_map(gaia::slam::destination_t&, gaia::slam::area_map_t&, - gaia::slam::observed_area_t&); +// Generates/updates path map. +void update_navigation_map(); // Checks to see if it's time to select a new destination. If so, the // destination record is updated and the function returns 'true'. Otherwise @@ -62,7 +59,7 @@ bool reassess_destination(); // Updates the observed_area record to make sure it includes all observed // areas plus the destination. -void update_world_area(gaia::slam::ego_t& ego); +void update_world_area(); // Instructs hardware layer to move toward destination. void move_toward_destination(); diff --git a/slam_sim/src/globals.cpp b/slam_sim/src/globals.cpp index 0d5df0a..a284e1b 100644 --- a/slam_sim/src/globals.cpp +++ b/slam_sim/src/globals.cpp @@ -10,6 +10,7 @@ #include +#include "constants.hpp" #include "occupancy.hpp" namespace slam_sim @@ -19,6 +20,8 @@ int32_t g_quit = 0; world_coordinate_t g_position = { .x_meters = 0.0, .y_meters = 0.0 }; float g_heading_degs = 0.0f; +occupancy_grid_t g_navigation_map(c_navigation_map_node_width_meters); + std::vector g_destinations; uint32_t g_next_destination = 0; diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 83f84af..809a47d 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -29,7 +29,7 @@ namespace slam_sim using std::this_thread::sleep_for; -static constexpr uint32_t c_rule_wait_millis = 200; +static constexpr uint32_t c_rule_wait_millis = 50; /** @@ -38,24 +38,28 @@ static constexpr uint32_t c_rule_wait_millis = 200; void main_loop() { int32_t ctr = 0; + g_running = true; // When the simulation completes it will set g_quit to 1. Then we can // exit. In the meantime, the simulation is being handled by rules. while (g_quit == 0) { if (g_running) { + update_world_area(); + update_navigation_map(); move_toward_destination(); + if (reassess_destination()) + { + g_quit = 1; + } // export_map_to_file(); build_export_map(); - if (ctr++ > 40) + if (ctr++ > 150) { g_quit = 1; } } - else - { - sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); - } + sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); } } @@ -77,7 +81,6 @@ void clean_db() { gaia::db::begin_transaction(); remove_all_rows(); - remove_all_rows(); remove_all_rows(); remove_all_rows(); remove_all_rows(); diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 3c097fd..6ffd0c0 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -14,13 +14,14 @@ #include #include +#include #include #include #include -#include "blob_cache.hpp" +//#include "blob_cache.hpp" #include "constants.hpp" #include "globals.hpp" #include "line_segment.hpp" @@ -35,18 +36,11 @@ using std::vector; using std::string; using std::cerr; -using gaia::slam::vertices_t; +using gaia::slam::destination_t;; +using gaia::slam::observed_area_t;; using gaia::slam::positions_t; using gaia::slam::range_data_t; - -using gaia::slam::area_map_t;; -//using gaia::slam::working_map_t;; -using gaia::slam::observed_area_t;; - -using gaia::slam::area_map_writer; - -blob_cache_t g_area_blobs; -blob_cache_t g_working_blobs; +using gaia::slam::vertices_t; //////////////////////////////////////////////////////////////////////// @@ -55,11 +49,11 @@ blob_cache_t g_working_blobs; void occupancy_grid_t::init(float node_width_meters, world_coordinate_t bottom_left, float width_meters, float height_meters) { + // Physcal size (width, height) of map grids. + m_node_size_meters = node_width_meters; // Number of grids in map. m_grid_size.rows = (uint32_t) ceil(height_meters / node_width_meters); m_grid_size.cols = (uint32_t) ceil(width_meters / node_width_meters); - // Physcal size (width, height) of map grids. - m_node_size_meters = node_width_meters; // Physical size of map. m_map_size.x_meters = width_meters; m_map_size.y_meters = height_meters; @@ -68,98 +62,51 @@ void occupancy_grid_t::init(float node_width_meters, } -void occupancy_grid_t::allocate_own_grid() +void occupancy_grid_t::allocate_grid(bool realloc_buffer) { - // Signal that 'this' owns the memory allocation. - m_blob_id = -1; uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); + if (realloc_buffer && (m_grid != NULL)) + { + m_grid = (map_node_t*) realloc(m_grid, num_nodes * sizeof *m_grid); + } + else + { + m_grid = (map_node_t*) malloc(num_nodes * sizeof *m_grid); + } } -// Creates new uncached map. -occupancy_grid_t::occupancy_grid_t(float node_width_meters, - world_coordinate_t bottom_left, float width_meters, float height_meters) +// Creates empty map. +occupancy_grid_t::occupancy_grid_t(float node_width_meters) { - init(node_width_meters, bottom_left, width_meters, height_meters); - allocate_own_grid(); - clear(); + world_coordinate_t bottom_left = { .x_meters = 0.0f, .y_meters = 0.0f }; + init(node_width_meters, bottom_left, 0.0, 0.0); + m_grid = NULL; } -// Loads existing map. -occupancy_grid_t::occupancy_grid_t(area_map_t& am) +occupancy_grid_t::occupancy_grid_t(float node_width_meters, + world_coordinate_t bottom_left, float width_meters, float height_meters) { - world_coordinate_t bottom_left = { - .x_meters = am.left_meters(), - .y_meters = am.bottom_meters() - }; - init(c_area_map_node_width_meters, bottom_left, - am.right_meters() - am.left_meters(), - am.top_meters() - am.bottom_meters()); - // Recover memory from blob cache. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - m_blob_id = am.blob_id();; - // Fetch existing blob. - blob_t* blob = g_area_blobs.get_blob(m_blob_id); - if (blob == NULL) - { - // Blob doesn't exist yet. This means that the process was - // started up against an existing database. This shouldn't - // happen, but allow it. Create it. - gaia_log::app().warn("Area map with ID {} did not have cached " - "blob. Recreating one.", m_blob_id); - size_t sz = num_nodes * sizeof *m_grid; - blob = g_area_blobs.create_blob(m_blob_id, sz); - m_grid = (map_node_t*) blob->data(); - // New grid. Nodes need initialization. - clear(); - } else { - assert(m_grid_size.rows == am.num_rows()); - assert(m_grid_size.cols == am.num_cols()); - m_grid = (map_node_t*) blob->data(); - // Existing grid. Should be initialized already. - } + init(node_width_meters, bottom_left, width_meters, height_meters); + allocate_grid(); + initialize_grid(); } -// Overwrites existing cached area map. -occupancy_grid_t::occupancy_grid_t(area_map_t& am, observed_area_t& area) +void occupancy_grid_t::reset(world_coordinate_t bottom_left, + float width_meters, float height_meters) { - world_coordinate_t bottom_left = { - .x_meters = am.left_meters(), - .y_meters = am.bottom_meters() - }; - init(c_area_map_node_width_meters, bottom_left, - area.right_meters() - area.left_meters(), - area.top_meters() - area.bottom_meters()); - // Get rid of old content and create a new empty blob. - uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; - uint32_t old_blob_id = am.blob_id(); - size_t sz = num_nodes * sizeof *m_grid; - m_blob_id = old_blob_id + 1; - blob_t* blob = g_area_blobs.create_blob(m_blob_id, sz, old_blob_id); - m_grid = (map_node_t*) blob->data(); - clear(); - // Store changes in DB. - area_map_writer writer = am.writer(); - writer.blob_id = m_blob_id; - writer.left_meters = area.left_meters(); - writer.right_meters = area.right_meters(); - writer.top_meters = area.top_meters(); - writer.bottom_meters = area.bottom_meters(); - writer.num_rows = m_grid_size.rows; - writer.num_cols = m_grid_size.cols; - writer.update_row(); -printf("BUILDING NEW MAP %d 0x%08lx\n", m_blob_id, (uint64_t) m_grid); + init(m_node_size_meters, bottom_left, width_meters, height_meters); + allocate_grid(true); + initialize_grid(); } occupancy_grid_t::~occupancy_grid_t() { - if (m_blob_id < 0) + if (m_grid) { - // Memory is owned by this. free(m_grid); } } @@ -193,9 +140,8 @@ void map_node_flags_t::clear() } -void occupancy_grid_t::clear() +void occupancy_grid_t::initialize_grid() { -printf("OCCUPANCY GRID CLEAR\n"); for (uint32_t y=0; y 0.0f) && (h_cost < cost)) { - // Horiz node is passable and penalty is less than via vert, so - // use horiz's penalty - cost = h_cost; - } - } - assert(cost >= 0.0); - // Cost needed to traverse to neighboring node. Scale it up because - // distance is farther (it's on a diagonal). - float traversal_weight = cost * 1.41f; - add_node_to_stack(root_idx, offset, traversal_weight); + add_node_to_stack(root_idx, offset, 1.41f); } // pixel neighbors @@ -345,7 +322,7 @@ void occupancy_grid_t::compute_path_costs() // stepping between adjacent nodes) out by several nodes, and measuing // the bearing to that node. If the path turns sharply (e.g., around // an obstacle) then the path is only traced to the turning point. -printf("---------------------Checking direction\n"); +//printf("---------------------Checking direction\n"); for (uint32_t y=0; y %d,%d is %.1f degs 0x%08lx\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, root.direction_degs, (uint64_t) &m_grid[idx]); +//printf("%d,%d -> %d,%d is %.1f degs 0x%08lx\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, root.direction_degs, (uint64_t) &m_grid[idx]); } } } @@ -414,6 +392,7 @@ void occupancy_grid_t::add_anchor_to_path_stack( { assert(idx.idx < m_grid_size.cols * m_grid_size.rows); map_node_t& path_node = m_grid[idx.idx]; +printf("Anchor point %d,%d, weight %f\n", path_node.pos.x, path_node.pos.y, path_weight); path_node.path_cost = path_weight; path_node.parent_idx.idx = c_invalid_grid_idx; m_queue.push(idx); @@ -514,6 +493,14 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination) compute_path_costs(); //printf("Computed costs\n"); //count_bounds(); +for (uint32_t y=0; y Date: Sun, 1 May 2022 12:04:20 -0700 Subject: [PATCH 27/30] first demo --- slam_sim/data/maze.json | 4 +-- slam_sim/include/txn.hpp | 54 ++++++++++++++++++++++++++++++++++++++ slam_sim/src/Makefile | 4 +-- slam_sim/src/main.cpp | 5 +++- slam_sim/src/occupancy.cpp | 12 +++++++-- slam_sim/src/path_map.cpp | 14 ---------- 6 files changed, 72 insertions(+), 21 deletions(-) create mode 100644 slam_sim/include/txn.hpp diff --git a/slam_sim/data/maze.json b/slam_sim/data/maze.json index 1fc828f..789af58 100644 --- a/slam_sim/data/maze.json +++ b/slam_sim/data/maze.json @@ -25,8 +25,8 @@ { "x": 6.1, "y": 4.9 }, { "x": 8.9, "y": 4.9 }, { "x": 8.9, "y": -4.9 }, - { "x": 0.1, "y": -4.9 }, - { "x": 0.1, "y": 4.9 } + { "x": -8.9, "y": -4.9 }, + { "x": -8.9, "y": 4.9 } ] }, { diff --git a/slam_sim/include/txn.hpp b/slam_sim/include/txn.hpp new file mode 100644 index 0000000..9290d92 --- /dev/null +++ b/slam_sim/include/txn.hpp @@ -0,0 +1,54 @@ +/////////////////////////////////////////////////////////////////////// +// Copyright (c) Gaia Platform LLC +// +// Use of this source code is governed by the MIT +// license that can be found in the LICENSE.txt file +// or at https://opensource.org/licenses/MIT. +/////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////// +// +// Transaction wrapper. Some API code that accesses the database +// may need to be called from both within rules and from main(). As +// there are no nested transactions, a transaction is open when calling +// from within a rule, and it's not automatically open when called +// procedurally, a smart wrapper for transactions is needed in the API +// that can begin/commit only if a transaction isn't already open. +// +// It is of course possible to require that code calling the API be +// aware of what the API functions require, but that can result in +// convoluted code, and it also works against code compartmentalization. +// +// This simple wrapper detects if a transaction is already open. If so, +// a user's call to 'begin()' will be ignored, as will any subsequent +// call to 'commit()'. +// +// There is no action on destruction -- it is entirely up to the user to +// call 'commit()'. Likewise, it is up to the user to catch any +// transaction errors. +// +/////////////////////////////////////////////////////////////////////// + +#pragma once + +namespace slam_sim +{ + +class txn_t +{ +public: + txn_t(); + ~txn_t() { ; } + + // Returns true if command was actually executed (i.e., if an existing + // transaction was not already open). + bool begin(); + bool commit(); + +protected: + //bool m_existing_transaction; + bool m_manage_txn; +}; + +} // namespace slam_sim + diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index a734e0a..5d5fdf7 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -28,8 +28,8 @@ test_analyze: test_analyze.cpp analyze.cpp $(CORE_OBJS) run: - ./$(TARGET) -m ../data/maze2.json -x -3.5 -y 1.5 - #./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 + #./$(TARGET) -m ../data/maze2.json -x -3.5 -y 1.5 + ./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 #./$(TARGET) -m ../data/map.json -x -3.0 -y 3.0 drun: diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 809a47d..4a9f077 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -23,6 +23,9 @@ #include "globals.hpp" #include "slam_sim.hpp" +// To make video of output files, use, e.g.: +// ffmpeg -r 5 -i export_%03d.pnm out.mp4 + namespace slam_sim { @@ -54,7 +57,7 @@ int32_t ctr = 0; } // export_map_to_file(); build_export_map(); - if (ctr++ > 150) + if (ctr++ > 250) { g_quit = 1; } diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 6ffd0c0..9553151 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -397,8 +397,16 @@ void occupancy_grid_t::export_as_pnm(string file_name) destination_t dest = *(destination_t::list().begin()); const map_node_t dest_node = get_node(dest.x_meters(), dest.y_meters()); uint32_t dest_idx = dest_node.pos.x + dest_node.pos.y * m_grid_size.cols; - b[dest_idx] = 255; - g[dest_idx] = 128; + for (int32_t y=-1; y<=1; y++) + { + int32_t row_idx = dest_idx + y * m_grid_size.cols; + for (int32_t x=-1; x<=1; x++) + { + int32_t idx = x + row_idx; + b[idx] = 255; + g[idx] = 128; + } + } txn.commit(); // Export image. diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 0037197..119a413 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -483,24 +483,10 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination) m_grid[idx].path_cost = 0.0f; } } -//printf("TRACE ROUTES grid at 0x%08lx (%d)\n", (uint64_t) m_grid, m_blob_id); -//count_bounds(); grid_index_t idx = get_node_index(destination.x_meters, destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); -//printf("Added anchor\n"); -//count_bounds(); compute_path_costs(); -//printf("Computed costs\n"); -//count_bounds(); -for (uint32_t y=0; y Date: Tue, 3 May 2022 10:42:25 -0700 Subject: [PATCH 28/30] pre-PR cleanup --- slam_sim/.gitignore | 1 + slam_sim/Makefile.inc | 10 +- slam_sim/data/maze3.json | 57 +++++++++ slam_sim/gaia/Makefile | 6 +- slam_sim/gaia/slam.ddl | 64 +--------- slam_sim/include/blob_cache.hpp | 90 ------------- slam_sim/include/constants.hpp | 16 +-- slam_sim/include/txn.hpp | 26 ++-- slam_sim/src/Makefile | 27 ++-- slam_sim/src/{utils => }/line_segment.cpp | 0 slam_sim/src/main.cpp | 19 ++- slam_sim/src/{rule_api.cpp => navigation.cpp} | 90 ++++++------- slam_sim/src/occupancy.cpp | 63 ++++----- slam_sim/src/path_map.cpp | 55 ++++---- slam_sim/src/support.cpp | 85 +++---------- slam_sim/src/test_analyze.cpp | 4 +- .../src/{utils => }/test_line_segment.cpp | 0 slam_sim/src/utils/Makefile | 28 ---- slam_sim/src/utils/README_blob_cache.md | 61 --------- slam_sim/src/utils/blob_cache.cpp | 103 --------------- slam_sim/src/utils/test_blob_cache.cpp | 114 ----------------- slam_sim/tests/test_analyze.cpp | 120 ------------------ slam_sim/tests/test_blob_cache.cpp | 63 --------- slam_sim/tests/test_line_segment.cpp | 70 ---------- 24 files changed, 230 insertions(+), 942 deletions(-) create mode 100644 slam_sim/data/maze3.json delete mode 100644 slam_sim/include/blob_cache.hpp rename slam_sim/src/{utils => }/line_segment.cpp (100%) rename slam_sim/src/{rule_api.cpp => navigation.cpp} (80%) rename slam_sim/src/{utils => }/test_line_segment.cpp (100%) delete mode 100644 slam_sim/src/utils/Makefile delete mode 100644 slam_sim/src/utils/README_blob_cache.md delete mode 100644 slam_sim/src/utils/blob_cache.cpp delete mode 100644 slam_sim/src/utils/test_blob_cache.cpp delete mode 100644 slam_sim/tests/test_analyze.cpp delete mode 100644 slam_sim/tests/test_blob_cache.cpp delete mode 100644 slam_sim/tests/test_line_segment.cpp diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index ba0ce4d..1fd335b 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -11,3 +11,4 @@ test_analyze test_blob_cache test_line_segment z* +*mp4 diff --git a/slam_sim/Makefile.inc b/slam_sim/Makefile.inc index 6e48f1f..aa38330 100644 --- a/slam_sim/Makefile.inc +++ b/slam_sim/Makefile.inc @@ -2,10 +2,12 @@ ROOT := $(dir $(lastword $(MAKEFILE_LIST))) WFLAGS = -Wall -Werror DEBUG_FLAG = -g -#OPTIMIZATION_FLAG = -O3 + +# Disable optimization for now -- using it generates a segfault at +# gaia/slam/gaia_slam.cpp:734 +#OPTIMIZATION_FLAG = -O1 + MISC_FLAGS = --std=c++17 INC = -I/opt/gaia/include/ -I$(ROOT)gaia/ -I$(ROOT)gaia/slam/ -I$(ROOT)include/ -LIB_SLAM_UTILS = $(ROOT)src/utils/slam_utils.a - -CPPFLAGS = $(WFLAGS) $(DEBUG_FLAG) $(OPTIMIZATION_FLAG) $(MISC_FLAGS) $(INC) +CPPFLAGS = $(WFLAGS) $(DEBUG_FLAG) $(MISC_FLAGS) $(INC) diff --git a/slam_sim/data/maze3.json b/slam_sim/data/maze3.json new file mode 100644 index 0000000..24b7f5d --- /dev/null +++ b/slam_sim/data/maze3.json @@ -0,0 +1,57 @@ +{ + "description": "Square maze", + "width": 15.0, + "height": 15.0, + "world": [ + { + "name": "Walls", + "vertices": [ + { "x": 0.1, "y": 0.1}, + { "x": 14.9, "y": 0.1}, + { "x": 14.9, "y": 2.9}, + { "x": 11.9, "y": 2.9}, + { "x": 11.9, "y": 11.9}, + { "x": 9.1, "y": 11.9}, + { "x": 9.1, "y": 2.9}, + { "x": 8.9, "y": 2.9}, + { "x": 8.9, "y": 11.9}, + { "x": 2.9, "y": 11.9}, + { "x": 2.9, "y": 12.1}, + { "x": 12.1, "y": 12.1}, + { "x": 12.1, "y": 3.1}, + { "x": 14.9, "y": 3.1}, + { "x": 14.9, "y": 14.9}, + { "x": 0.1, "y": 14.9}, + { "x": 0.1, "y": 0.1} + ] + }, + { + "name": "Island", + "vertices": [ + { "x": 2.9, "y": 2.9}, + { "x": 6.1, "y": 2.9}, + { "x": 6.1, "y": 3.1}, + { "x": 2.9, "y": 3.1}, + { "x": 2.9, "y": 2.9} + ] + }, + { + "name": "Cup", + "vertices": [ + { "x": 2.9, "y": 5.9}, + { "x": 3.1, "y": 5.9}, + { "x": 3.1, "y": 8.9}, + { "x": 5.9, "y": 8.9}, + { "x": 5.9, "y": 5.9}, + { "x": 6.1, "y": 5.9}, + { "x": 6.1, "y": 9.1}, + { "x": 2.9, "y": 9.1}, + { "x": 2.9, "y": 5.9} + ] + } + ], + "destinations": [ + { "x": 13.5, "y": 4.5 }, + { "x": 4.5, "y": 7.5 } + ] +} diff --git a/slam_sim/gaia/Makefile b/slam_sim/gaia/Makefile index 2d40a8d..e23e49c 100644 --- a/slam_sim/gaia/Makefile +++ b/slam_sim/gaia/Makefile @@ -3,13 +3,15 @@ include ../Makefile.inc RULES_CPP = slam_rules.cpp DB_NAME = slam -OBJS = slam_rules.o slam/gaia_slam.o +OBJS = slam/gaia_slam.o +#OBJS = slam_rules.o slam/gaia_slam.o + all: all_gaia all_procedure_subshell all_gaia: gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME) - gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) + #gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) # when freshly making, slam_rules.cpp may not exist (e.g., if 'make clean' diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index aa524e6..b97e602 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -10,17 +10,13 @@ -- -- Schema for SLAM simulation. -- --- Schema is split into 3 categories of tables. +-- Schema is split into 2 categories of tables. -- -- The first includes tables that have individual records in them. For -- example, the 'ego', the 'destination', and maps that represent the -- area (used for path finding). -- --- The second includes tables storing event data. This includes destination --- requests pushed in externally (e.g., an external request for Alice --- to go to a particular position) or if Alice senses a collision. --- --- The third includes the main elements of the SLAM algorithm. This includes +-- The second includes the main elements of the SLAM algorithm, such as -- graphs, edges, and vertices, and auxiliary tables to support these. -- ------------------------------------------------------------------------ @@ -30,14 +26,9 @@ database slam; -- Table list -- Single record tables: -- ego Stores state of the bot. ----- area_map Map of the known world. -- destination Location that bot is moving to. -- observed_area Bounds of observed world. -- --- Event tables: --- pending_destination Destination requested externally. --- collision_event Sensors detected imminent collision. --- -- Data tables: -- graphs Group of observations to produce pose graph. -- vertices Where an observation is made (aka: pose, node). @@ -50,7 +41,9 @@ database slam; -- Note that 'vertex' and 'observation' are often used interchangeaby. -- In future, these should be split. An 'observation' is a view of the -- environment, and that may or may not become a vertex, while a vertex --- will always be associated with an observation. +-- will always be associated with an observation. An observation seems +-- to be called KeyPoint by some. + ------------------------------------------------------------------------ -- Tables with single records (i.e., exactly one) @@ -80,9 +73,6 @@ table ego -- Position oriented. destination references destination, world references observed_area - --- -- Map oriented. --- map references area_map ) @@ -101,29 +91,6 @@ table observed_area ) ----- Maps would ideally store map content as blobs but max blob size is ----- presently too small. Instead, store pointer to current map and ----- manage concurrent access manually. ---table area_map ---( ---// -- Reference to in-memory blob that stores 2D map. ---// -- Blob ID changes on each map update. ---// blob_id uint32, ---// --- -- Bounding polygon --- -- Uses world coordinates, with increasing X,Y being rightward/upward. --- left_meters float, --- right_meters float, --- top_meters float, --- bottom_meters float, --- -- Grid size --- num_rows uint32, --- num_cols uint32, --- --- ego references ego ---) - - table destination ( -- Uses world coordinates, with increasing X,Y being rightward/upward. @@ -154,27 +121,6 @@ table latest_observation ) ------------------------------------------------------------------------- --- Event table(s) --- Pending actions should be put in an event table, as well as unexpected --- events. - -table pending_destination -( - -- When a destination request is made, it is stored here. When it's - -- appropriate, this location will become the the new destination. - -- Uses world coordinates, with increasing X,Y being rightward/upward. - x_meters float, - y_meters float -) - - -table collision_event -( - description string -) - - ------------------------------------------------------------------------ -- Data tables (i.e., tables with multiple records) diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp deleted file mode 100644 index f3964bf..0000000 --- a/slam_sim/include/blob_cache.hpp +++ /dev/null @@ -1,90 +0,0 @@ -/////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -/////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////// -// -// Mechanism for memory 'blobs' to be accessed by the database. -// -// These blobs are on the heap. A reference number (ID) is stored in -// a database table and that ID is used to fetch a blob. Blobs are -// not persistent and their content must be regenerated from database -// content if they are not available. -// -// Blob creation and destruction is designed to be resistant to -// transaction rollbacks and retries. See the README. -// -/////////////////////////////////////////////////////////////////////// - -#pragma once - -#include -#include - -#include -#include - -namespace slam_sim -{ - -constexpr uint32_t c_invalid_blob_id = 0; - - -class blob_t -{ -public: - blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob); - ~blob_t(); - - uint32_t id() { return m_id; } - size_t size() { return m_size; } - uint8_t* data() { return m_data; } - - uint32_t supersedes() { return m_id_superseded_blob; } - -protected: - friend class blob_cache_t; - - uint32_t m_id; - - // Blob data information. - size_t m_size; - uint8_t* m_data; - - // Blob that this blob supersedes. - // It will be deleted when this blob itself is superseded. - uint32_t m_id_superseded_blob; -}; - - -class blob_cache_t -{ -public: - blob_cache_t() = default; - blob_cache_t(const blob_cache_t&) = delete; - blob_cache_t& operator=(const blob_cache_t&) = delete; - - // Creates a blob with the specified ID and characteristics - blob_t* create_blob(uint32_t id, size_t size, - uint32_t id_superseded_blob = c_invalid_blob_id); - - // Retrieves a blob given an id. - // Will return nullptr if the blob doesn't exist. - blob_t* get_blob(uint32_t id); - -protected: - // Map of allocated blobs. - std::unordered_map m_blob_map; - std::shared_mutex m_lock; - - // Nested get_blob() call from w/in create_or_reset_blob(), where a - // mutex lock has already been established. - blob_t* get_blob_(uint32_t id); -}; - -} // namespace slam_sim - diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp index dfcb7dc..fe1782b 100644 --- a/slam_sim/include/constants.hpp +++ b/slam_sim/include/constants.hpp @@ -23,22 +23,20 @@ constexpr float c_export_map_node_width_meters = 0.05f; // Path finding. // Bias factor for seeking out new areas to explore. This is the increase -// in cost to traverse a node for each time it's been observed. -constexpr float c_path_penalty_per_observation = 0.05f; +// in cost to traverse a node for each time it's been occupied. +constexpr float c_path_penalty_per_occupation = 0.5f; // The Dijkstra-like path finding algorithms are very rough on the local // scale, having at most 8 directions to move away from a node in the map. -// The approach here to smooth that out is to look at the path necessary -// to trace out X nodes from a given location and use that to generate -// the directional vector from each node. The constant here is that X. +// The approach here is to smooth the path direction by looking ahead +// X nodes and using this direction necessar to go to that node +// to generate the direction vector. The constant here is that X. constexpr uint32_t c_num_ancestors_for_direction = 5; -// Distance between each step. Keyframes are taken after several steps. +// Distance travelled between each simulated step. constexpr float c_step_meters = 0.8 * c_navigation_map_node_width_meters; -constexpr uint32_t c_num_steps_between_keyframes = 1; - -// How close to destination bot has to declare it's reached it. +// How close to destination bot has to be to declare victory. constexpr float c_destination_radius_meters = 0.5f; } // namespace slam_sim diff --git a/slam_sim/include/txn.hpp b/slam_sim/include/txn.hpp index 9290d92..85d72bb 100644 --- a/slam_sim/include/txn.hpp +++ b/slam_sim/include/txn.hpp @@ -8,23 +8,17 @@ /////////////////////////////////////////////////////////////////////// // -// Transaction wrapper. Some API code that accesses the database -// may need to be called from both within rules and from main(). As -// there are no nested transactions, a transaction is open when calling -// from within a rule, and it's not automatically open when called -// procedurally, a smart wrapper for transactions is needed in the API -// that can begin/commit only if a transaction isn't already open. +// Transaction wrapper. Transactions cannot presently be nested. This +// means that each function accessing the database must be aware of +// where it's being called from, and whether there's an existing +// transaction open or not. To eliminate the boilerplate in each +// function, a transaction wrapper is used. Begin and commit calls +// are made to the wrapper, and the wrapper keeps track of whether +// an existing transaction is open or not. If so, the begin/commit +// are ignored as those will be part of the outer transaction. // -// It is of course possible to require that code calling the API be -// aware of what the API functions require, but that can result in -// convoluted code, and it also works against code compartmentalization. -// -// This simple wrapper detects if a transaction is already open. If so, -// a user's call to 'begin()' will be ignored, as will any subsequent -// call to 'commit()'. -// -// There is no action on destruction -- it is entirely up to the user to -// call 'commit()'. Likewise, it is up to the user to catch any +// There is no action on destruction -- it is entirely up to the user +// to call 'commit()'. Likewise, it is up to the user to catch any // transaction errors. // /////////////////////////////////////////////////////////////////////// diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 5d5fdf7..935ab4f 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -1,7 +1,7 @@ include ../Makefile.inc -# Make sure that we're not using g++. If so then environment variables -# aren't correctly set. +# Make sure that we're not using g++. Use that as a proxy to determine +# if environment variables are set, as we should be using clang if so. GXX_NAME = g++ ifeq ($(CXX), $(GXX_NAME)) $(error Using g++. Environment variables are not correctly configured) @@ -11,13 +11,13 @@ endif TARGET = slam_sim TEST_TARGETS = test_analyze -CORE_OBJS = analyze.o occupancy.o rule_api.o path_map.o support.o globals.o txn.o +CORE_OBJS = analyze.o occupancy.o navigation.o path_map.o support.o \ + globals.o txn.o line_segment.o OBJS = main.o $(CORE_OBJS) -EXT_OBJS = ../gaia/slam_rules.o ../gaia/gaia_slam.o -LIBS = $(LIB_SLAM_UTILS) /usr/local/lib/libgaia.so -pthread -lcurl +EXT_OBJS = ../gaia/gaia_slam.o +LIBS = /usr/local/lib/libgaia.so -pthread -lcurl -all: $(OBJS) - cd utils && make +all: $(OBJS) $(CXX) $(OBJS) $(EXT_OBJS) -o $(TARGET) $(LIBS) test: $(TEST_TARGETS) @@ -28,22 +28,21 @@ test_analyze: test_analyze.cpp analyze.cpp $(CORE_OBJS) run: + ./$(TARGET) -m ../data/maze3.json -x 4.5 -y 6.0 #./$(TARGET) -m ../data/maze2.json -x -3.5 -y 1.5 - ./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 - #./$(TARGET) -m ../data/map.json -x -3.0 -y 3.0 + #./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 drun: - gdb --args ./slam_sim -m ../data/maze.json -x -4.5 -y 1.25 - #gdb --args ./slam_sim -m ../data/map.json -x -3.0 -y 3.0 + gdb --args ./slam_sim -m ../data/maze3.json -x 4.5 -y 6.0 + #gdb --args ./slam_sim -m ../data/maze.json -x -4.5 -y 1.25 -%.o: %.cpp - $(CXX) $(INC) -c $(CPPFLAGS) $< +%.o: %.cpp + $(CXX) $(INC) -c $(CPPFLAGS) $(OPTIMIZATION_FLAG) $< refresh: clean all clean: - cd utils && make clean rm -f *.o $(TARGET) *.pnm rm -rf logs diff --git a/slam_sim/src/utils/line_segment.cpp b/slam_sim/src/line_segment.cpp similarity index 100% rename from slam_sim/src/utils/line_segment.cpp rename to slam_sim/src/line_segment.cpp diff --git a/slam_sim/src/main.cpp b/slam_sim/src/main.cpp index 4a9f077..dd8e548 100644 --- a/slam_sim/src/main.cpp +++ b/slam_sim/src/main.cpp @@ -41,6 +41,7 @@ static constexpr uint32_t c_rule_wait_millis = 50; void main_loop() { int32_t ctr = 0; +int32_t reached_destinations = 0; g_running = true; // When the simulation completes it will set g_quit to 1. Then we can // exit. In the meantime, the simulation is being handled by rules. @@ -53,11 +54,17 @@ int32_t ctr = 0; move_toward_destination(); if (reassess_destination()) { - g_quit = 1; + reached_destinations++; + if (reached_destinations >= 3) + { + g_quit = 1; + } + } + if (ctr & 1) + { + build_export_map(); } -// export_map_to_file(); - build_export_map(); - if (ctr++ > 250) + if (ctr++ > 1200) { g_quit = 1; } @@ -86,8 +93,6 @@ void clean_db() remove_all_rows(); remove_all_rows(); remove_all_rows(); - remove_all_rows(); - remove_all_rows(); remove_all_rows(); remove_all_rows(); remove_all_rows(); @@ -158,7 +163,7 @@ void parse_command_line(int argc, char** argv) { usage(argc, argv); } - gaia_log::app().info("Initial possition at {},{}", + gaia_log::app().info("Initial possition at {},{}", g_position.x_meters, g_position.y_meters); gaia_log::app().info("Loading world map {}", map_file); load_world_map(map_file); diff --git a/slam_sim/src/rule_api.cpp b/slam_sim/src/navigation.cpp similarity index 80% rename from slam_sim/src/rule_api.cpp rename to slam_sim/src/navigation.cpp index 28b2b2e..a30d212 100644 --- a/slam_sim/src/rule_api.cpp +++ b/slam_sim/src/navigation.cpp @@ -45,33 +45,11 @@ using gaia::slam::destination_writer; // Determine if a new graph optimization is necessary. // In a live example, this function would apply logic to determine if // enough data has been collected (e.g., new range data, new closures, -// etc.) to justify performing another map optimization. For now, say -// an optiimzation is required every X vertices. +// etc.) to justify performing another map optimization. For now, say +// an optiimzation is required every step. bool optimization_required() { return true; -// static int32_t ctr = 0; -// if (ctr < 4) -// { -// return true; -// } -// else if (ctr < 8) -// { -// return (ctr & 1) == 0; -// } -// else if (ctr < 16) -// { -// return (ctr & 3) == 0; -// } -// else if (ctr < 32) -// { -// return (ctr & 7) == 0; -// } -// else -// { -// return (ctr & 15) == 0; -// } -// return false; } @@ -150,21 +128,6 @@ bool need_to_extend_map(positions_t& pos, observed_area_t& bounds) } -//static void build_map(area_map_t& am) -//{ -// static int32_t ctr = 0; -// occupancy_grid_t map(am); -// // There are C++ ways to format a number to a string (e.g., -// // fmt::format or std::stringstream, but the C approach is -// // nice in its simplicity. -//// TODO get timestamp to use for name -// char fname[256]; -// sprintf(fname, "map_%03d.pnm", ctr++); -// gaia_log::app().info("Building map {}", fname); -// map.export_as_pnm(fname); -//} - - void export_map_to_file() { static int32_t ctr = 0; @@ -189,14 +152,14 @@ void build_export_map() }; float width_meters = region.right_meters() - region.left_meters(); float height_meters = region.top_meters() - region.bottom_meters(); - // Each time we build an area map - occupancy_grid_t map(c_export_map_node_width_meters, + // Each time we build an area map + occupancy_grid_t map(c_export_map_node_width_meters, bottom_left, width_meters, height_meters); for (graphs_t& g: graphs_t::list()) { for (vertices_t& v: g.vertices()) { - //gaia_log::app().info("Pulling sensor data from {}:{}", + //gaia_log::app().info("Pulling sensor data from {}:{}", // g.id(), v.id()); map.apply_sensor_data(v); } @@ -270,7 +233,8 @@ bool reassess_destination() { // Select next destination. world_coordinate_t new_dest = g_destinations[g_next_destination++]; -printf("Next destination: %.1f,%.1f\n", new_dest.x_meters, new_dest.y_meters); + gaia_log::app().info("Next desination is {},{}", new_dest.x_meters, + new_dest.y_meters); if (g_next_destination >= g_destinations.size()) { g_next_destination = 0; @@ -286,5 +250,45 @@ printf("Next destination: %.1f,%.1f\n", new_dest.x_meters, new_dest.y_meters); return rc; } + +// Infrastructure has info on latest position so no need to query DB +// (infra is what sent that info to the DB). +void move_toward_destination() +{ + txn_t txn; + txn.begin(); + // Move forward one step. + // Get direction to head from map. + grid_index_t idx = g_navigation_map.get_node_index(g_position.x_meters, + g_position.y_meters); + map_node_t node = g_navigation_map.get_node(idx); + //map_node_t& node = g_navigation_map.get_node(g_position.x_meters, + // g_position.y_meters); + float heading_degs = node.direction_degs; + // Move in that direction. + float dist_meters = c_step_meters; + float s, c; + sincosf(c_deg_to_rad * heading_degs, &s, &c); + float dx_meters = s * dist_meters; + float dy_meters = c * dist_meters; + g_position.x_meters += dx_meters; + g_position.y_meters += dy_meters; + g_heading_degs = heading_degs; + gaia_log::app().info("Moving {},{} meters to {},{}", dx_meters, + dy_meters, g_position.x_meters, g_position.y_meters); + // Position moved. Wait a bit before proceeding, to account for + // at least a little bit of travel time. + usleep(50000); + // TODO Add logic to determine when keyframes should be created and + // when to convert those to a vertex. E.g., does this location have + // salient features? does it provide sensor data that's new? + // For now, create a vertex every time we've moved forward a small + // amount. + create_vertex(g_position, g_heading_degs); + // + txn.commit(); +} + + } // namespace slam_sim diff --git a/slam_sim/src/occupancy.cpp b/slam_sim/src/occupancy.cpp index 9553151..40dd680 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -21,7 +21,6 @@ #include -//#include "blob_cache.hpp" #include "constants.hpp" #include "globals.hpp" #include "line_segment.hpp" @@ -94,7 +93,7 @@ occupancy_grid_t::occupancy_grid_t(float node_width_meters, } -void occupancy_grid_t::reset(world_coordinate_t bottom_left, +void occupancy_grid_t::reset(world_coordinate_t bottom_left, float width_meters, float height_meters) { init(m_node_size_meters, bottom_left, width_meters, height_meters); @@ -159,7 +158,7 @@ void occupancy_grid_t::initialize_grid() // Node access -// Returns index of map node at specified location. Location uses +// Returns index of map node at specified location. Location uses grid_index_t occupancy_grid_t::get_node_index(float x_meters, float y_meters) { // x index. @@ -171,7 +170,7 @@ grid_index_t occupancy_grid_t::get_node_index(float x_meters, float y_meters) } // y index. float bottom_inset_meters = y_meters - m_bottom_left.y_meters;; - uint32_t y_idx = + uint32_t y_idx = (uint32_t) floor(bottom_inset_meters / m_node_size_meters); if (y_idx >= m_grid_size.rows) { @@ -214,7 +213,7 @@ world_coordinate_t occupancy_grid_t::get_node_position(grid_coordinate_t& pos) assert(pos.x < m_grid_size.cols); assert(pos.y < m_grid_size.rows); world_coordinate_t coord; - coord.x_meters = m_bottom_left.x_meters + coord.x_meters = m_bottom_left.x_meters + ((float) pos.x + 0.5f) * m_node_size_meters; coord.y_meters = m_bottom_left.y_meters + m_map_size.y_meters - (((float) pos.y + 0.5f) * m_node_size_meters); @@ -224,7 +223,7 @@ world_coordinate_t occupancy_grid_t::get_node_position(grid_coordinate_t& pos) //////////////////////////////////////////////////////////////////////// // Applying sensor data to the map grids -// Updates occupancy, oberved and boundary flags in map from +// Updates occupancy, oberved and boundary flags in map from // observations on this radial. // Steps through radial and estimates what grid squares it crosses. void occupancy_grid_t::apply_radial(float radial_degs, float range_meters, @@ -232,13 +231,13 @@ void occupancy_grid_t::apply_radial(float radial_degs, float range_meters, { // Set observed and boundary flags. // Measured distance on radial. - double dist_meters = range_meters < 0.0 + double dist_meters = (range_meters < 0.0) ? c_range_sensor_max_meters : range_meters; float s, c; sincosf(c_deg_to_rad * radial_degs, &s, &c); - // Number of points on radial to examine. Sample at a fraction of + // Number of points on radial to examine. Sample at a fraction of // the grid size. - uint32_t num_steps = (uint32_t) + uint32_t num_steps = (uint32_t) ceil(dist_meters / (m_node_size_meters / 3.0)); double step_size_meters = dist_meters / num_steps; for (uint32_t i=1; i<=num_steps; i++) @@ -295,9 +294,9 @@ void occupancy_grid_t::apply_flags() // First 8 deltas are in first ring around node. Those // are ADJ_IMPASSABLE. Second ring are CLOSE_IMPASSABLE. // Third ring are NEAR_IMPASSABLE. - const int32_t x_delta[48] = { + const int32_t x_delta[48] = { -1, 0, 1, -1, 1, -1, 0, 1, - -2, -1, 0, 1, 2, -2, 2, -2, + -2, -1, 0, 1, 2, -2, 2, -2, 2, -2, 2, -2, -1, 0, 1, 2, -3, -2, -1, 0, 1, 2, 3, -3, 3, -3, 3, -3, 3, -3, 3, -3, 3, @@ -305,34 +304,34 @@ void occupancy_grid_t::apply_flags() }; const int32_t y_delta[48] = { -1, -1, -1, 0, 0, 1, 1, 1, - -2, -2, -2, -2, -2, -1, -1, 0, + -2, -2, -2, -2, -2, -1, -1, 0, 0, 1, 1, 2, 2, 2, 2, 2, - -3, -3, -3, -3, -3, -3, -3, + -3, -3, -3, -3, -3, -3, -3, -2, -2, -1, -1, 0, 0, 1, 1, 2, 2, - 3, -3, 3, 3, 3, 3, 3 + 3, -3, 3, 3, 3, 3, 3 }; for (uint32_t i=0; i<48; i++) { - uint32_t nbr_x = (uint32_t) + uint32_t nbr_x = (uint32_t) ((int32_t) node.pos.x + x_delta[i]); - uint32_t nbr_y = (uint32_t) + uint32_t nbr_y = (uint32_t) ((int32_t) node.pos.y + y_delta[i]); if ((nbr_x < m_grid_size.cols) && (nbr_y < m_grid_size.rows)) { uint32_t nbr_idx = nbr_x + nbr_y * m_grid_size.cols; if (i < 8) { - m_grid[nbr_idx].flags.state |= + m_grid[nbr_idx].flags.state |= PATH_NODE_FLAG_ADJ_IMPASSABLE; } else if (i < 24) { - m_grid[nbr_idx].flags.state |= + m_grid[nbr_idx].flags.state |= PATH_NODE_FLAG_CLOSE_IMPASSABLE; } - else + else { - m_grid[nbr_idx].flags.state |= + m_grid[nbr_idx].flags.state |= PATH_NODE_FLAG_NEAR_IMPASSABLE; } } @@ -353,7 +352,7 @@ void occupancy_grid_t::apply_sensor_data(const vertices_t& obs) // Apply sensor data. for (int32_t i=0; i 0.0f) -// { -////printf("%d,%d (%dx%d=%d) bounds %f\n", x, y, m_grid_size.cols, m_grid_size.rows, idx, m_grid[idx].boundary); -// cnt++; -// } -// } -// } -// printf("Boundary count: %d\n", cnt); -//} - } // namespace slam_sim diff --git a/slam_sim/src/path_map.cpp b/slam_sim/src/path_map.cpp index 119a413..ad5219e 100644 --- a/slam_sim/src/path_map.cpp +++ b/slam_sim/src/path_map.cpp @@ -10,6 +10,9 @@ /*********************************************************************** * Path-finding logic for occupancy grid. Code here adapted from * <> +* +* This is a support file for occupancy.cpp. +* ***********************************************************************/ #include @@ -145,7 +148,8 @@ static struct drand48_data path_rand_; // (eg, weight and link to parent) // Add node to stack for future neighbor analysis. // 'traversal_cost' is the cost to reach this node from the neighbor that -// added it to the stack. +// added it to the stack. That should be 1.0 for 4-connected neighbors +// and 1.4 for diagonally connected. void occupancy_grid_t::add_node_to_stack( /* in */ const grid_index_t root_idx, /* in */ const node_offset_t offset, @@ -166,17 +170,17 @@ void occupancy_grid_t::add_node_to_stack( // Point is in the world. Check it. grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; map_node_t& child_node = m_grid[new_idx.idx]; -//printf("Adding child %d,%d travers %f root cost %f\n", new_x, new_y, traversal_cost, root_node.path_cost); if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) { -//printf(" child %d,%d impassable\n", new_x, new_y); return; } // Determine added weight for traversing this node. Have lower weight - // to nodes infrequently observed/traversed to provide bias to take + // to nodes less frequently traversed to provide bias to take // different routes. - //float weight = child_node.observed * c_path_penalty_per_observation; - float weight = 0.0; + float weight = child_node.occupied * c_path_penalty_per_occupation; + // Increase the weight for nodes that are impassable or are near + // impassable nodes. In practice, impassable weight should be much + // higher, but this value is OK for the size of maps used presently. // We don't need to add boundary consideration here because the above // conditional checking impassable filtered out grid squares where // a boundary existed. Add a sanity check to make sure. @@ -194,7 +198,7 @@ void occupancy_grid_t::add_node_to_stack( weight += 5.0; } // Path tracing algorithm is deterministic and can follow vert or - // horiz path too easily. Add some jitter to allow pulling in + // horiz paths too easily. Add some jitter to allow pulling in // from more accurate direction. double jitter = 0.0; drand48_r(&path_rand_, &jitter); @@ -205,20 +209,17 @@ void occupancy_grid_t::add_node_to_stack( // it a lower cost. if (child_node.path_cost <= new_cost) { -//printf(" new cost %f is above existing cost %f\n", new_cost, child_node.path_cost); // Nope. return; } - // New weight is lower -- allow node to be added to stack again to + // New weight is lower -- allow node to be added to stack again to // propagate updated weight to neighbors. } -//printf("child %d,%d flags %02x weight %f\n", new_x, new_y, child_node.flags.state, weight); // Add point to stack for future consideration. child_node.parent_idx = root_idx; child_node.path_cost = new_cost; child_node.traversal_cost = weight; child_node.flags.state |= PATH_NODE_FLAG_PROCESSED; -//printf("%d,%d cost %f\n", new_x, new_y, new_cost); m_queue.push(new_idx); } @@ -238,8 +239,8 @@ void occupancy_grid_t::add_node_to_stack_diag( const map_node_t& root_node = m_grid[root_idx.idx]; int32_t new_x = (int32_t) root_node.pos.x + offset.dx; int32_t new_y = (int32_t) root_node.pos.y + offset.dy; - if ((new_x < 0) || (new_x >= m_grid_size.cols) - || (new_y < 0) || (new_y >= m_grid_size.rows)) + if ((new_x < 0) || (new_x >= m_grid_size.cols) + || (new_y < 0) || (new_y >= m_grid_size.rows)) { return; } @@ -260,7 +261,7 @@ void occupancy_grid_t::add_node_to_stack_diag( (uint32_t) (new_x + root_node.pos.y * m_grid_size.cols); const map_node_t& vert_child_node = m_grid[idx_vert]; const map_node_t& horiz_child_node = m_grid[idx_horiz]; - uint8_t pass_mask = + uint8_t pass_mask = PATH_NODE_FLAG_IMPASSABLE | PATH_NODE_FLAG_NEAR_IMPASSABLE; if ((vert_child_node.flags.state & pass_mask) && (horiz_child_node.flags.state & pass_mask)) @@ -314,15 +315,14 @@ void occupancy_grid_t::compute_path_costs() { process_next_stack_node(); } - // Now follow gradient in each node to build directional vector + // Now follow gradient in each node to build directional vector // to reach destination. pixel_offset_bitfield_t base_direction, next_direction; // Iterate through nodes and build direction vector for each. Vector - // is defined by following the path toward the destination (i.e., + // is defined by following the path toward the destination (i.e., // stepping between adjacent nodes) out by several nodes, and measuing // the bearing to that node. If the path turns sharply (e.g., around // an obstacle) then the path is only traced to the turning point. -//printf("---------------------Checking direction\n"); for (uint32_t y=0; y %d,%d is %.1f degs 0x%08lx\n", root.pos.x, root.pos.y, ggp.pos.x, ggp.pos.y, root.direction_degs, (uint64_t) &m_grid[idx]); } } } @@ -392,7 +388,6 @@ void occupancy_grid_t::add_anchor_to_path_stack( { assert(idx.idx < m_grid_size.cols * m_grid_size.rows); map_node_t& path_node = m_grid[idx.idx]; -printf("Anchor point %d,%d, weight %f\n", path_node.pos.x, path_node.pos.y, path_weight); path_node.path_cost = path_weight; path_node.parent_idx.idx = c_invalid_grid_idx; m_queue.push(idx); @@ -406,7 +401,7 @@ world_coordinate_t occupancy_grid_t::get_node_location( world_coordinate_t world_pos; assert(pos.x < m_grid_size.cols); assert(pos.y < m_grid_size.rows); - world_pos.x_meters = m_bottom_left.x_meters + + world_pos.x_meters = m_bottom_left.x_meters + ((float) pos.x + 0.5) * m_node_size_meters; world_pos.y_meters = m_bottom_left.y_meters + m_map_size.y_meters - ((float) pos.y + 0.5) * m_node_size_meters; @@ -428,7 +423,7 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination, } } // Add anchors. First destination, then boundary conditions. - grid_index_t idx = get_node_index(destination.x_meters, + grid_index_t idx = get_node_index(destination.x_meters, destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); // For each boundary point, get distance/weight from parent map at @@ -442,14 +437,14 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination, world_coordinate_t world_pos = get_node_location(pos); // It's assumed that this map is a subset of parent map and // is fully contained in it as that's a design constraint. - map_node_t& parent_node = parent_map.get_node(world_pos.x_meters, + map_node_t& parent_node = parent_map.get_node(world_pos.x_meters, world_pos.y_meters); grid_index_t idx = { .idx = y * m_grid_size.cols }; add_anchor_to_path_stack(idx, parent_node.path_cost); // Last column. pos.x = m_grid_size.rows - 1; world_pos = get_node_location(pos); - parent_node = parent_map.get_node(world_pos.x_meters, + parent_node = parent_map.get_node(world_pos.x_meters, world_pos.y_meters); idx.idx = y * m_grid_size.cols + (m_grid_size.cols - 1); add_anchor_to_path_stack(idx, parent_node.path_cost); @@ -483,7 +478,7 @@ void occupancy_grid_t::trace_routes(world_coordinate_t& destination) m_grid[idx].path_cost = 0.0f; } } - grid_index_t idx = get_node_index(destination.x_meters, + grid_index_t idx = get_node_index(destination.x_meters, destination.y_meters); add_anchor_to_path_stack(idx, 0.0f); compute_path_costs(); diff --git a/slam_sim/src/support.cpp b/slam_sim/src/support.cpp index 6b9ceee..b17ecf2 100644 --- a/slam_sim/src/support.cpp +++ b/slam_sim/src/support.cpp @@ -20,8 +20,11 @@ #include #include +#include "gaia_slam.h" + #include "constants.hpp" #include "globals.hpp" +#include "occupancy.hpp" #include "slam_sim.hpp" #include "txn.hpp" @@ -36,9 +39,10 @@ using std::max; using gaia::common::gaia_id_t; -using gaia::slam::edges_t; using gaia::slam::destination_t; +using gaia::slam::edges_t; using gaia::slam::ego_t; +using gaia::slam::error_corrections_t; using gaia::slam::graphs_t; using gaia::slam::latest_observation_t; using gaia::slam::movements_t; @@ -51,11 +55,6 @@ using gaia::slam::latest_observation_writer; using gaia::slam::observed_area_writer; -//////////////////////////////////////////////////////////////////////// -// Background API that supports the contents of rule_api.cpp. Most -// importantly, this means that the functions here are expected to -// be called from within an active transaction - // Updates the observed_area record to make sure it includes all observed // areas plus the destination. void update_world_area() @@ -105,16 +104,18 @@ void update_world_area() oa_writer.top_meters = top_edge; oa_writer.update_row(); } -printf("World area: %.1f,%.1f to %.1f,%.1f\n", left_edge, bottom_edge, right_edge, top_edge); + gaia_log::app().info("World area is now {},{} to {},{}", left_edge, + bottom_edge, right_edge, top_edge); txn.commit(); -} +} void create_vertex(world_coordinate_t pos, float heading_degs) { -printf("Creating vertex\n"); + txn_t txn; + txn.begin(); uint32_t obs_num = s_next_vertex_id++; - gaia_log::app().info("Performing observation {} at {},{} heading {}", + gaia_log::app().info("Performing observation {} at {},{} heading {}", obs_num, pos.x_meters, pos.y_meters, heading_degs); sensor_data_t data; map_coord_t coord = { @@ -127,7 +128,7 @@ printf("Creating vertex\n"); // Get ego ego_t& ego = *(ego_t::list().begin()); uint32_t graph_id = ego.current_graph_id(); - + // Get most recent obesrvation. vertices_t prev_vert = ego.latest_observation().vertex(); float prev_x_meters, prev_y_meters, prev_heading_degs; @@ -201,65 +202,17 @@ printf("Creating vertex\n"); latest_observation_writer lo_writer = ego.latest_observation().writer(); lo_writer.vertex_id = obs_num; lo_writer.update_row(); -} - - -//////////////////////////////////////////////////////////////////////// -// Non-rule API -// The functions here must manage their own transactions. - -// Infrastructure has info on latest position so no need to query DB -// (infra is what sent that info to the DB). -void move_toward_destination() -{ -printf("MOVING\n"); - txn_t txn; - txn.begin(); - // Make several small steps. - for (uint32_t i=0; i 1); - // Area map and observed area start out with same dimensions. + // Area map and observed area start out with same dimensions. // Shorthand. float max_range = c_range_sensor_max_meters; // Align calls laterally for visual inspection to help make sure variables @@ -296,8 +249,8 @@ void seed_database(float x_meters, float y_meters) new_dest.y_meters, // y_meters 0.0 // departure_time_sec ); -printf("initial destination %f,%f\n", new_dest.x_meters, new_dest.y_meters); - + gaia_log::app().info("Initial destination is {},{}", new_dest.x_meters, + new_dest.y_meters); //////////////////////////////////////////// // Relationships ego.destination().connect(destination_id); @@ -306,7 +259,7 @@ printf("initial destination %f,%f\n", new_dest.x_meters, new_dest.y_meters); //////////////////////////////////////////// // All done - gaia::db::commit_transaction(); + txn.commit(); } } // namespace slam_sim diff --git a/slam_sim/src/test_analyze.cpp b/slam_sim/src/test_analyze.cpp index a84afe5..a2bfc0c 100644 --- a/slam_sim/src/test_analyze.cpp +++ b/slam_sim/src/test_analyze.cpp @@ -42,7 +42,7 @@ static uint32_t check_radial(uint32_t num, double expected, if (fabs(data.range_meters[num] - expected) > 0.01) { fprintf(stderr, "Radial %d (%.1f degs) has range of %.3f, " - "expected %.3f\n", num, data.bearing_degs[num], + "expected %.3f\n", num, data.bearing_degs[num], data.range_meters[num], expected); errs++; } @@ -62,7 +62,7 @@ int main(int argc, char** argv) load_world_map(WORLD_MAP_FILE); std::string response; sensor_data_t data; - map_coord_t coord = { + map_coord_t coord = { .x_meters = -3.0, .y_meters = 4.4, .heading_degs = 45.0 diff --git a/slam_sim/src/utils/test_line_segment.cpp b/slam_sim/src/test_line_segment.cpp similarity index 100% rename from slam_sim/src/utils/test_line_segment.cpp rename to slam_sim/src/test_line_segment.cpp diff --git a/slam_sim/src/utils/Makefile b/slam_sim/src/utils/Makefile deleted file mode 100644 index dcf127c..0000000 --- a/slam_sim/src/utils/Makefile +++ /dev/null @@ -1,28 +0,0 @@ -include ../../Makefile.inc - -TEST_TARGETS = test_line_segment test_blob_cache - -OBJS = line_segment.o blob_cache.o - - -all: $(OBJS) - ar rcs $(LIB_SLAM_UTILS) *.o - -test: $(TEST_TARGETS) - -test_line_segment: line_segment.cpp - $(CXX) $(CPPFLAGS) test_line_segment.cpp line_segment.cpp -o test_line_segment - -test_blob_cache: blob_cache.cpp - $(CXX) $(CPPFLAGS) test_blob_cache.cpp blob_cache.cpp -o test_blob_cache - - -%.o: %.cpp - $(CXX) $(INC) -c $(CPPFLAGS) $< - -refresh: clean all - -clean: - rm -f *.o *.a $(TEST_TARGETS) - - diff --git a/slam_sim/src/utils/README_blob_cache.md b/slam_sim/src/utils/README_blob_cache.md deleted file mode 100644 index bd28458..0000000 --- a/slam_sim/src/utils/README_blob_cache.md +++ /dev/null @@ -1,61 +0,0 @@ -# A mechanism for storing non-persistent memory blobs. - -Must support blob allocation during a transaction and be resistent -to memory leaks when transactions are rolled back and retried. - -Primary use case is creating a memory allocation that stores non-primary -data that's used during other computations. Non-primary means that the -data in the blob can be (re)generated anytime from content stored in the -database. - -If a blob exists it can be used by the calling process. -If it doesn't exist then the calling process is responsible for -initializing it. - -One need for such a blob is for storing map data in SLAM, as navigation -requires a large blob that represents obstacles and routes to a destination. - -The database stores an ID for the blob, and sends that ID to the blob -factory, asking for the memory associated w/ that blob. If the blob doesn't -exist it is created. The factory needs to communicate to the calling process -whether the blob existed or if it was freshly created. -This is done by using get() to fetch the blob. If it exists a valid pointer -to it is returned. Otherwise NULL is returned, in which case the calling -processes needs to call create(). If create() is called and -the blob with a given ID already exists, then the existing blob is returned. -The calling function will not necessarily know if it's a new or existing blob, -although it's guaranteed that a new blob will have its data field initialized -to NULL (e.g., uint8\_t\* data = new uint8\_t\[size]]();) - - -If blobs are created and destroyed, there must be a safe way to delete old -ones, and this must operate in a world where a delete call may be issued -many times (e.g., if called repeatedly during retried transactions) and -should be tolerant of a delete being ignored (e.g., failed transaction -that's not retried). - -This is achieved by each blob having a reference to a blob that it -supersedes, so if A is allocated and B is meant to supersede A (e.g., -because of a resize), then a pointer to A is stored within B. -Even though A is superseded, it remains a valid memory allocation until -B is superseded. - -Only when B is superseded will A be deleted. - -If B is allocated but the transaction is rolled back and not retried, -then A will remain in use. B will be allocated but won't be used until -the calling process asks the blob factory for a blob w/ B's ID. - -As noted earlier, the calling process must manage the IDs for memory. -A simple way to do this is to store the blob ID in a table. When creating a new blob during a transaction, increment the ID and ask the factory for a blob with this new ID. -If the transaction is rolled back, the newly created blob will have been allocated but the ID will not be -incremented, so if it's retried it will request a blob with the same ID again. -That request can simultaneously have the former ID be superseded. -E.g.,: -``` - blob_t* blob = cache.create_blob( - ID+1, // new blob ID - size_in_bytes, // size of memory to be allocated - ID // superceded blob - ); -``` diff --git a/slam_sim/src/utils/blob_cache.cpp b/slam_sim/src/utils/blob_cache.cpp deleted file mode 100644 index 6ef4076..0000000 --- a/slam_sim/src/utils/blob_cache.cpp +++ /dev/null @@ -1,103 +0,0 @@ -//////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////// - -#include "blob_cache.hpp" -#include "retail_assert.hpp" - -namespace slam_sim -{ - -blob_t::blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob) -{ - RETAIL_ASSERT( - size > 0, - "Attempted to allocate a blob of 0 size!"); - - m_id = id; - m_size = size; - m_id_superseded_blob = id_superseded_blob; - m_data = new uint8_t[size](); -} - - -blob_t::~blob_t() -{ - RETAIL_ASSERT( - m_data != nullptr, - "Deleting blob NULL data element. Was it double deleted?"); - - delete[] m_data; - m_data = nullptr; -} - -//////////////////////////////////////////////////////////////////////// - -blob_t* blob_cache_t::create_blob(uint32_t id, size_t size, - uint32_t id_superseded_blob) -{ - if ((size == 0) || (id == c_invalid_blob_id)) - { - return nullptr; - } - - std::unique_lock unique_lock(m_lock); - - // Lookup superseded blob. If it exists, and if it itself has a - // superseded blob, then delete the latter. - if (id_superseded_blob != c_invalid_blob_id) - { - blob_t* superseded_blob = nullptr; - superseded_blob = get_blob_(id_superseded_blob); - if (superseded_blob != nullptr) - { - uint32_t corpse_id = superseded_blob->supersedes(); - if (corpse_id != c_invalid_blob_id) { - // Delete corpse and remove all references to it. - blob_t* corpse = get_blob_(corpse_id); - delete corpse; - m_blob_map[corpse_id] = nullptr; - superseded_blob->m_id_superseded_blob = c_invalid_blob_id; - } - - } - } - - // Check if a blob with this id already exists. If so, reuse it. If not, - // create a new one and insert it into our map. - blob_t* blob = get_blob_(id); - if (blob == nullptr) - { - blob = new blob_t(id, size, id_superseded_blob); - m_blob_map.insert(std::pair(id, blob)); - } - return blob; -} - - -blob_t* blob_cache_t::get_blob(uint32_t id) -{ - if (id == c_invalid_blob_id) - { - return nullptr; - } - std::shared_lock shared_lock(m_lock); - return get_blob_(id); -} - - -blob_t* blob_cache_t::get_blob_(uint32_t id) -{ - RETAIL_ASSERT( - id != c_invalid_blob_id, - "Attempting to retrieve a blob using an invalid id!"); - - return (m_blob_map.count(id) > 0) ? m_blob_map[id] : nullptr; -} - -} // namespace slam_sim - diff --git a/slam_sim/src/utils/test_blob_cache.cpp b/slam_sim/src/utils/test_blob_cache.cpp deleted file mode 100644 index 247bd9c..0000000 --- a/slam_sim/src/utils/test_blob_cache.cpp +++ /dev/null @@ -1,114 +0,0 @@ -//////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////// - -#include - -#include - -#include "blob_cache.hpp" -#include "retail_assert.hpp" - -using namespace std; - -namespace slam_sim -{ - -static int32_t test_blob_cache() -{ - uint32_t errs = 0; - blob_cache_t cache; - // Create blob. - blob_t* blob1 = cache.create_blob(1, 16, 0); - if (blob1 == nullptr) - { - fprintf(stderr, "Failed to create blob 1\n"); - errs++; - } - if (cache.get_blob(1) == nullptr) - { - fprintf(stderr, "Failed to fetch blob 1\n"); - errs++; - } - - // Create blob 2, superseding 1. - blob_t* blob2 = cache.create_blob(2, 16, 1); - if (blob2 == nullptr) - { - fprintf(stderr, "Failed to create blob 2\n"); - errs++; - } - if (cache.get_blob(2) == nullptr) - { - fprintf(stderr, "Failed to fetch blob 2\n"); - errs++; - } - - // Make sure 1's still around - if (cache.get_blob(1) == nullptr) - { - fprintf(stderr, "Blob 1 delete prematurely\n"); - errs++; - } - - // Create blob 3, superseding 2, which should eliminate 1 - blob_t* blob3 = cache.create_blob(3, 16, 2); - if (blob3 == nullptr) - { - fprintf(stderr, "Failed to create blob 3\n"); - errs++; - } - if (cache.get_blob(3) == nullptr) - { - fprintf(stderr, "Failed to fetch blob 3\n"); - errs++; - } - // Make sure 2's still around - if (cache.get_blob(2) == nullptr) - { - fprintf(stderr, "Blob 2 delete prematurely\n"); - errs++; - } - // Make sure 1 is gone - if (cache.get_blob(1) != nullptr) - { - fprintf(stderr, "Blob 1 was not deleted\n"); - errs++; - } - - // Try to create invalid blob. - blob_t* blob0 = cache.create_blob(0, 16, 0); - if (blob0 != nullptr) - { - fprintf(stderr, "Succeeded in creating invalid blob\n"); - errs++; - } - - //////////////////////////////////////////////////////////////////// - return errs; -} - -} // namespace slam_sim - - -int main(void) -{ - uint32_t errs = 0; - errs += slam_sim::test_blob_cache(); - //////////////////////////////////////////////////////////////////// - if (errs > 0) - { - fprintf(stderr, "********************************\n"); - fprintf(stderr, "Encountered %d errors\n", errs); - } - else - { - fprintf(stderr, "All tests pass\n"); - } - return static_cast(errs); -} - diff --git a/slam_sim/tests/test_analyze.cpp b/slam_sim/tests/test_analyze.cpp deleted file mode 100644 index 666ae69..0000000 --- a/slam_sim/tests/test_analyze.cpp +++ /dev/null @@ -1,120 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -#include -#include -#include -#include - -#include - -#include "json.hpp" -#include "line_segment.hpp" -#include "sensor_data.hpp" -#include "slam_sim.hpp" - -constexpr const char* WORLD_MAP_FILE = "../data/map.json"; - -using namespace slam_sim; -using namespace utils; - -void print_sensor_data(sensor_data_t& data) -{ - double step_degs = 360.0 / (double) data.num_radials; - for (uint32_t i=0; i 0.01) - { - fprintf(stderr, "Radial %d (%d degs) has range of %.3f, " - "expected %.3f\n", num, 2*num, data.range_meters[num], expected); - errs++; - } - return errs; -} - - -int main(int argc, char** argv) -{ - (void) argc; - (void) argv; - uint32_t errs = 0; - - gaia::system::initialize(); - - //////////////////////////////////////////////////////////////////// - load_world_map(WORLD_MAP_FILE); - std::string response; - sensor_data_t data; - perform_sensor_sweep(-3.0, -4.4, data); - - // wall distances - errs += check_radial(0, 0.5, data); - errs += check_radial(22, 0.7, data); - errs += check_radial(45, -1.0, data); - errs += check_radial(90, -1.0, data); - errs += check_radial(134, 3.9, data); - - // table distances - // 190 degs. Strikes vertical edge of table near bottom. - // sqrt(0.5^2 + (0.5/tan(10))^2) - errs += check_radial(95, 2.879, data); - - // 196 degs. Strikes horizontal edge of table near right edge. - // sqrt(1.9^2 + (1.9*tan(16))^2) - errs += check_radial(98, 1.977, data); - - // 216 degs. Strikes horizontal edge of table near left edge. - // sqrt(1.9^2 + (1.9*tan(36))^2) - errs += check_radial(108, 2.349, data); - - // Check landmarks. - if (data.landmarks_visible.size() != 1) - { - fprintf(stderr, "Expected seeing 1 landmark but saw %d\n", - (int32_t) data.landmarks_visible.size()); - errs++; - } - else - { - if (data.landmarks_visible[0].id != 1) - { - fprintf(stderr, "Expected seeing landmark #1 but saw %d\n", - data.landmarks_visible[0].id); - errs++; - } - } - - //////////////////////////////////////////////////////////////////// - if (errs > 0) - { - fprintf(stderr, "********************************\n"); - fprintf(stderr, "Encountered %d errors\n", errs); - } - else - { - fprintf(stderr, "All tests pass\n"); - } - - return static_cast(errs); -} diff --git a/slam_sim/tests/test_blob_cache.cpp b/slam_sim/tests/test_blob_cache.cpp deleted file mode 100644 index 1a2a439..0000000 --- a/slam_sim/tests/test_blob_cache.cpp +++ /dev/null @@ -1,63 +0,0 @@ -//////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////// - -#include - -#include - -#include "blob_cache.hpp" -#include "retail_assert.hpp" - -using namespace std; - -void test_blob_cache(); - -int main(void) -{ - test_blob_cache(); -} - -void test_blob_cache() -{ - // Create two blobs. - blob_cache_t::get()->create_or_reset_blob(1, 1, 0); - blob_cache_t::get()->create_or_reset_blob(2, 1, 1); - - // Read back the information of the blobs that we created. - blob_t* blob_ptr = blob_cache_t::get()->get_blob(1); - cout << "Blob 1 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - blob_ptr = blob_cache_t::get()->get_blob(2); - cout << "Blob 2 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - // Create a third blob. It should lead to the deletion of the first blob. - blob_cache_t::get()->create_or_reset_blob(3, 1, 2); - - blob_ptr = blob_cache_t::get()->get_blob(3); - cout << "Blob 3 has superseded blob with id '" << blob_ptr->id_superseded_blob << "'" << endl; - - // Also update this blob's state. - blob_ptr->state = blob_state_t::used; - cout << "Blob 3 state is now '" << (size_t)blob_ptr->state << "'" << endl; - - // Verify that the first blob has been deleted. - blob_ptr = blob_cache_t::get()->get_blob(1); - RETAIL_ASSERT( - !blob_ptr, "FAILED: Blob 1 still exists!"); - cout << "Blob 1 has been removed, as expected!" << endl; - - // Reset the third blob state. - blob_cache_t::get()->create_or_reset_blob(3, 1, 2); - - // Read the third blob and verify that its state has been reset. - blob_ptr = blob_cache_t::get()->get_blob(3); - RETAIL_ASSERT( - blob_ptr->state == blob_state_t::initialized, - "FAILED: Blob 3 state has not been reset!"); - cout << "Blob 3 has been reset, as expected!" << endl; -} diff --git a/slam_sim/tests/test_line_segment.cpp b/slam_sim/tests/test_line_segment.cpp deleted file mode 100644 index 9a2e69c..0000000 --- a/slam_sim/tests/test_line_segment.cpp +++ /dev/null @@ -1,70 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// - -#include -#include - -#include "line_segment.hpp" - -using namespace std; - -using utils::line_segment_t; - -uint32_t check_distance(double len, double expected) -{ - if (fabs(len - expected) > 0.001) - { - fprintf(stderr, "Distance out of range. Got %.3f, expected %.3f\n", - len, expected); - return 1; - } - return 0; -} - -int main() -{ - uint32_t errs = 0; - - // horizontal line at y=-10 (i.e., 10 units above the origin) - line_segment_t a(-10.0, -10.0, 10.0, -10.0); - errs += check_distance(a.intersect_range(0.0, 0.0, 0.0), 10.0); - errs += check_distance(a.intersect_range(0.0, 0.0, 30.0), 11.547); - errs += check_distance(a.intersect_range(0.0, 0.0, -30.0), 11.547); - errs += check_distance(a.intersect_range(0.0, 0.0, 60.0), -1.0); - errs += check_distance(a.intersect_range(0.0, 0.0, 150.0), -1.0); - errs += check_distance(a.intersect_range(0.0, 0.0, 270.0), -1.0); - - // diagonal line crossing from 0,-10 to 10,0 (i.e., 1st quadrant) - line_segment_t b(0.0, -10.0, 10.0, 0.0); - errs += check_distance(b.intersect_range(0.0, 0.0, 0.1), 9.983); - errs += check_distance(b.intersect_range(0.0, 0.0, -0.01), -1.0); - errs += check_distance(b.intersect_range(0.0, 0.0, 359.99), -1.0); - errs += check_distance(b.intersect_range(0.0, 0.0, 45.0), 7.071); - errs += check_distance(b.intersect_range(0.0, 0.0, 225.0), -1.0); - - // opposite orientation horizontal line at y=10 - line_segment_t c(10.0, -10.0, -10.0, -10.0); - errs += check_distance(c.intersect_range(0.0, 0.0, 0.0), 10.0); - errs += check_distance(c.intersect_range(0.0, 0.0, 30.0), 11.547); - errs += check_distance(c.intersect_range(0.0, 0.0, -30.0), 11.547); - errs += check_distance(c.intersect_range(0.0, 0.0, 60.0), -1.0); - errs += check_distance(c.intersect_range(0.0, 0.0, 150.0), -1.0); - errs += check_distance(c.intersect_range(0.0, 0.0, 270.0), -1.0); - - if (errs > 0) - { - fprintf(stderr, "********************************\n"); - fprintf(stderr, "Encountered %d errors\n", errs); - } - else - { - fprintf(stderr, "All tests pass\n"); - } - - return static_cast(errs); -} From 11fe41c37a1108d12290f2ddc7342a0f5a72eba5 Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Tue, 3 May 2022 10:59:22 -0700 Subject: [PATCH 29/30] Remove ruleset file --- slam_sim/gaia/slam.ruleset | 165 ------------------------------------- 1 file changed, 165 deletions(-) delete mode 100644 slam_sim/gaia/slam.ruleset diff --git a/slam_sim/gaia/slam.ruleset b/slam_sim/gaia/slam.ruleset deleted file mode 100644 index d55e442..0000000 --- a/slam_sim/gaia/slam.ruleset +++ /dev/null @@ -1,165 +0,0 @@ -//////////////////////////////////////////////////////////////////////// -// Copyright (c) Gaia Platform LLC -// -// Use of this source code is governed by the MIT -// license that can be found in the LICENSE.txt file -// or at https://opensource.org/licenses/MIT. -//////////////////////////////////////////////////////////////////////// -// -// Rules for performing SLAM in the slam simulator. -// -//////////////////////////////////////////////////////////////////////// - -#include -#include - -#include -#include - -#include -#include -#include - -#include "globals.hpp" -#include "slam_sim.hpp" - -/*********************************************************************** -Rules - -map_ruleset serial_group - on_insert(edges) If optimization required, do so and rebuild maps. - on_change(destination) Rebuild area map, move toward new destination. - on_insert(vertices) Extend area map if necessary. - -ruleset - on_insert(vertices) Spatial query to link to other nodes. - on_insert(vertices) Determine if it's time to change destination. - on_insert(collision_event) Full stop and create vertex. - - -***********************************************************************/ - -//using slam_sim::build_area_map; -using slam_sim::full_stop; -//using slam_sim::move_toward_destination; -//using slam_sim::need_to_extend_map; -//using slam_sim::optimize_graph; -//using slam_sim::optimization_required; -//using slam_sim::reassess_destination; -//using slam_sim::update_world_area; - -//using slam_sim::g_running; - - -// Wrap error correction and map creation in a serial group as these are -// expensive operations and we want to avoid concurrent operations -// experiencing a txn conflict, and avoid race conditions. -ruleset map_ruleset : serial_group() -{ -// // We don't want multiple edges to be created at similar points in time -// // and both edges realize that it's time to optimize the map, so keep -// // the graph optimization check in a serial group. -// on_insert(edges) -// { -//printf("1 enter on_insert(edges)\n"); -// // New edge created. See if it's time to run new graph optimization. -// if (optimization_required()) -// { -// optimize_graph(edges.dest->vertices.graph->graphs); -// // Whenever new error-correction data is available, regenerate -// // maps. This can be triggered in a separate rule or simply -// // done here, so do it here. -// build_area_map(/destination, /area_map, /observed_area); -// } -//printf("1 leave on_insert(edges)\n"); -// } -// -// -// // Destination changed. Update working map and start moving toward it. -// on_change(destination) -// { -//printf("2 enter on_change(destination)\n"); -// // Destination's changed. Make sure the known world includes -// // this point. -// update_world_area(/ego); -// // Build map, including paths, to destination. -// build_area_map(/destination, /area_map, /observed_area); -// // Start movement toward destination. -// g_running = true; -//printf("2 leave on_change(destination)\n"); -// } -// -// -// // For each vertex, update working map using new sensor data. -// on_insert(vertices) -// { -//printf("3 enter on_insert(vertices)\n"); -// // If working map will go beyond border of area map, rebuild -// // area map. -// if (need_to_extend_map(vertices.position->positions, /observed_area)) -// { -// build_area_map(/destination, /area_map, /observed_area); -// } -//printf("3 leave on_insert(vertices)\n"); -// } -} - - -ruleset runtime_ruleset -{ - // Vertices are created at a lower infrastructure level. The - // decision to do so is based on the following assumptions about - // when an observation is made: - // 1) Vertices are generated at locations that provide - // non-redundant information about the environment, such as range - // data that covers previously unexplored territory or being - // near salient landmark to form closures with other nearby - // vertices. - // 2) Sensor data is read at a rate higher than vertices are - // created. - // 3) The time required to evaluate whether or not to make an - // observation may be longer than the interval between successive - // packets of sensor data. - // This latter assumption means that one can't directly use a rule - // triggering of sensor data to build vertices as these must - // run in parallel, risking violation of (1), as other vertices - // made during processing will be invisible to the rule's txn. - - -// on_insert(vertices) -// { -// // TODO Perform spatial query in area of observation and see if -// // there are other vertices to build edges with (i.e., build -// // loop closures) -// } -// -// on_insert(vertices) -// { -//printf("4 enter on_insert(vertices)\n"); -// // Determine if it's time to change destinations. -// if (reassess_destination()) -// { -// // TODO If destination changes, start a new graph. -// } -//printf("4 leave on_insert(vertices)\n"); -// } - - on_insert(collision_event) - { -printf("5 enter on_insert(collision_event)\n"); - // A collision was detected, or the range sensor detected something - // closer than it's supposed to be. Initiate a stop, which will - // in turn create a new observation. - gaia_log::app().info("Collision detected. Stopping bot"); - full_stop(); -printf("5 leave on_insert(collision_event)\n"); - } - -// on_insert(graphs) -// { -//printf("6 enter on_insert(graphs)\n"); -// /ego.current_graph_id = graphs.id; -//printf("6 leave on_insert(graphs)\n"); -// } -} - From b20d9007c99df9babcdeda9e6cdc6c0d8e729f6c Mon Sep 17 00:00:00 2001 From: Keith Godfrey Date: Tue, 3 May 2022 11:09:05 -0700 Subject: [PATCH 30/30] More code cleanup --- slam_sim/include/globals.hpp | 3 --- slam_sim/include/occupancy.hpp | 7 ------- slam_sim/include/sensor_data.hpp | 2 -- slam_sim/include/slam_sim.hpp | 9 --------- slam_sim/src/Makefile | 6 +++++- slam_sim/src/analyze.cpp | 4 ---- slam_sim/src/line_segment.cpp | 9 --------- 7 files changed, 5 insertions(+), 35 deletions(-) diff --git a/slam_sim/include/globals.hpp b/slam_sim/include/globals.hpp index 97df0ec..92e7e53 100644 --- a/slam_sim/include/globals.hpp +++ b/slam_sim/include/globals.hpp @@ -24,9 +24,6 @@ extern occupancy_grid_t g_navigation_map; // to quit it's set to 1. extern int32_t g_quit; -//// Blob caches for area maps and working maps. -//extern blob_cache_t g_area_blobs; - // Bot's position. This is known at the 'infrastructure' level (e.g., ROS) // which is where position data is fed to the DB from. This is the current // (estimated) position of the bot. diff --git a/slam_sim/include/occupancy.hpp b/slam_sim/include/occupancy.hpp index 8440570..30ed59a 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -20,8 +20,6 @@ are increasing to the up and right, as elsewhere in the code. The map itself is stored in an array which uses the standard image coordinate system, with the first element in the array being at the top-left. The code has to manage the switch between the two. -// TODO FIXME verify that this is indeed the case, and that the switch -// is being properly handled. ***********************************************************************/ #include @@ -103,9 +101,6 @@ struct map_node_t // The grid (map) itself. class occupancy_grid_t { -//public: -// void count_bounds(); -// public: // Builds an empty map. This is for use when constructing global map. // Map remains uninitialized. To make it usable, call reset(). @@ -125,8 +120,6 @@ class occupancy_grid_t map_node_flags_t& get_node_flags(float x_meters, float y_meters); map_node_flags_t& get_node_flags(grid_index_t idx); -map_node_t* get_node_ptr(grid_index_t idx) { return &m_grid[idx.idx]; } - grid_index_t get_node_index(float pos_x_meters, float pos_y_meters); // Returns the center of the grid node at the specified coordinates. diff --git a/slam_sim/include/sensor_data.hpp b/slam_sim/include/sensor_data.hpp index dc59dda..123b189 100644 --- a/slam_sim/include/sensor_data.hpp +++ b/slam_sim/include/sensor_data.hpp @@ -31,8 +31,6 @@ constexpr double RANGE_SENSOR_MAX_METERS = 4.0; struct sensor_data_t { -// uint32_t num_radials; - // Range on each radial. Range is -1 if there's no data. std::vector range_meters; diff --git a/slam_sim/include/slam_sim.hpp b/slam_sim/include/slam_sim.hpp index aef3129..5e03369 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -43,12 +43,6 @@ bool optimization_required(); // to iterate through a graph's vertices and edges. void optimize_graph(gaia::slam::graphs_t&); - -//// Checks to see if known world has extended beyong map boundaries so much -//// that it's time to rebuild the area map. -//bool need_to_extend_map(gaia::slam::positions_t& pos, -// gaia::slam::observed_area_t& bounds); - // Generates/updates path map. void update_navigation_map(); @@ -91,9 +85,6 @@ void export_map_to_file(); // active graph. This is higher resolution than the navigatin map. void build_export_map(); -// External request to move to a specific location. -void request_destination(float x_meters, float y_meters); - //////////////////////////////////////////////// // Environmental analysis diff --git a/slam_sim/src/Makefile b/slam_sim/src/Makefile index 935ab4f..21291ac 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -9,7 +9,7 @@ endif TARGET = slam_sim -TEST_TARGETS = test_analyze +TEST_TARGETS = test_analyze test_line_segment CORE_OBJS = analyze.o occupancy.o navigation.o path_map.o support.o \ globals.o txn.o line_segment.o @@ -26,6 +26,10 @@ test_analyze: test_analyze.cpp analyze.cpp $(CORE_OBJS) $(CXX) $(CPPFLAGS) test_analyze.cpp -o test_analyze \ $(CORE_OBJS) $(EXT_OBJS) $(LIBS) +test_line_segment: test_line_segment.cpp line_segment.cpp $(CORE_OBJS) + $(CXX) $(CPPFLAGS) test_line_segment.cpp -o test_line_segment \ + $(CORE_OBJS) $(EXT_OBJS) $(LIBS) + run: ./$(TARGET) -m ../data/maze3.json -x 4.5 -y 6.0 diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index de557fe..35bc1b8 100644 --- a/slam_sim/src/analyze.cpp +++ b/slam_sim/src/analyze.cpp @@ -102,7 +102,6 @@ void load_world_map(const char* world_map) // making up the 2D world map. void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { -//printf("Calculating range data from %f,%f\n", coord.x_meters, coord.y_meters); data.range_meters.clear(); data.bearing_degs.clear(); float step_degs = c_range_sensor_sweep_degs / (c_num_range_radials - 1); @@ -114,7 +113,6 @@ void calculate_range_data(map_coord_t& coord, sensor_data_t& data) + (float) n * step_degs; theta_degs = theta_degs >= 360.0 ? theta_degs - 360.0 : theta_degs; theta_degs = theta_degs < 0.0 ? theta_degs + 360.0 : theta_degs; -//printf(" %d: %.1f\n", n, theta_degs); // Measure distance on this radial float min_meters = -1.0; int32_t line_num = -1; @@ -123,7 +121,6 @@ void calculate_range_data(map_coord_t& coord, sensor_data_t& data) line_segment_t& seg = g_world_lines[i]; float dist_meters = seg.intersect_range(coord.x_meters, coord.y_meters, theta_degs); -//printf(" %.1f,%.1f -> %.1f,%.1f range %.2f\n", seg.m_x0, seg.m_y0, seg.m_x1, seg.m_y1, dist_meters); if (dist_meters > 0.0) { if ((min_meters < 0.0) || (dist_meters < min_meters)) @@ -137,7 +134,6 @@ void calculate_range_data(map_coord_t& coord, sensor_data_t& data) { min_meters = -1.0; } -//printf(" range %.1f at %.1f\n", min_meters, theta_degs); data.range_meters.push_back(min_meters); data.bearing_degs.push_back(theta_degs); } diff --git a/slam_sim/src/line_segment.cpp b/slam_sim/src/line_segment.cpp index f926bc3..ddb649a 100644 --- a/slam_sim/src/line_segment.cpp +++ b/slam_sim/src/line_segment.cpp @@ -84,9 +84,6 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) double x4 = x3 + 10.0 * sin(D2R * theta); double y3 = y; double y4 = y + 10.0 * cos(D2R * theta); -//printf("This segment: %.1f,%.1f -> %.1f,%.1f\n", x1, y1, x2, y2); -//printf(" -> line %.1f,%.1f at %.1f\n", x3, y3, theta); -//printf(" becomes: %.1f,%.1f -> %.4f,%.4f\n", x3, y3, x4, y4); // Px = ((x1y2 - x2y1)(x3-x4) - (x1-x2)(x3y4-y3x4)) / denom; // Py = ((x1y2 - y1x2)(y3-y4) - (y1-y2)(x3y4-y3x4)) / denom; // denom = (x1-x2)(y3-y4) - (y1-y2)(x3-x4); @@ -104,7 +101,6 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) { double px = (xy12 * x3_x4 - xy34 * x1_x2) / denom; double py = (xy12 * y3_y4 - xy34 * y1_y2) / denom; -//printf(" intersect point %.4f,%.4f\n", px, py); // Intersection may or may not be in the direction of theta. // All this algorithm tells us is that the lines, infinitely // expanded, meet at this point. Find out if intersection @@ -119,7 +115,6 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) { delta_degs -= 360.0; } -//printf(" measured theta: %.1f delta: %f\n", inter_theta, delta_degs); if (fabs(delta_degs) < 1.0) { // Intersection point is in direction of theta. Now see if @@ -130,18 +125,14 @@ double line_segment_t::intersect_range(double x, double y, double theta_deg) double dist1 = measure_distance(px-x2, py-y2); // If longest distance is less than line segment length then // the intersection point is w/in the segment. -//printf("dist 0: %f\n", dist0); -//printf("dist 1: %f\n", dist1); if ((dist0 <= m_len) && (dist1 <= m_len)) { dist = measure_distance(px-x3, py-y3); } } } // Otherwise lines are parallel. -//printf(" range %.1f\n", dist); return dist; } - } // namespace slam_sim