diff --git a/slam_sim/.gitignore b/slam_sim/.gitignore index 80a8978..1fd335b 100644 --- a/slam_sim/.gitignore +++ b/slam_sim/.gitignore @@ -5,3 +5,10 @@ out2 slam_sim *.o logs +*.pgm +*.pnm +test_analyze +test_blob_cache +test_line_segment +z* +*mp4 diff --git a/slam_sim/Makefile b/slam_sim/Makefile index b0a9642..f165e58 100644 --- a/slam_sim/Makefile +++ b/slam_sim/Makefile @@ -1,17 +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/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/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/data/maze.json b/slam_sim/data/maze.json new file mode 100644 index 0000000..789af58 --- /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": -8.9, "y": -4.9 }, + { "x": -8.9, "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..f881adf --- /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": -2.1 }, + { "x": -1.9, "y": -2.1 }, + { "x": -1.9, "y": 3.9 }, + { "x": 1.9, "y": 3.9 }, + { "x": 1.9, "y": -2.1 }, + { "x": 2.1, "y": -2.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/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 6233bbf..e23e49c 100644 --- a/slam_sim/gaia/Makefile +++ b/slam_sim/gaia/Makefile @@ -3,17 +3,18 @@ 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: rules all_procedure_subshell -gaiac: - gaiac slam.ddl -g --db-name $(DB_NAME) -o $(DB_NAME) +all: all_gaia all_procedure_subshell -rules: gaiac - gaiat slam.ruleset -output $(RULES_CPP) -- -I/usr/lib/clang/10/include $(INC) +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, rules.cpp may not exist (e.g., if 'make clean' +# 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: diff --git a/slam_sim/gaia/slam.ddl b/slam_sim/gaia/slam.ddl index 509a088..b97e602 100644 --- a/slam_sim/gaia/slam.ddl +++ b/slam_sim/gaia/slam.ddl @@ -10,56 +10,75 @@ -- -- 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 '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 --- 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 --- observations (more commonly called poses) and sequences of --- observations here called paths (akin to simplified pose graphs). +-- The second includes the main elements of the SLAM algorithm, such as +-- graphs, edges, and vertices, and auxiliary tables to support these. -- ------------------------------------------------------------------------ database slam; --- unit suffixes --- meters --- degs (degress) --- sec (seconds) --- pct (percentage) +-- Table list +-- Single record tables: +-- ego Stores state of the bot. +-- destination Location that bot is moving to. +-- observed_area Bounds of observed world. +-- +-- Data tables: +-- graphs Group of observations to produce pose graph. +-- 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. An observation seems +-- to be called KeyPoint by some. ------------------------------------------------------------------------ -- 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, + + -- Most recent timestmap is stored in most recent observation/vertex + -- (referenced below). -- 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, - destination references destination, - error references error_correction, + -- 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, - low_res_map references area_map, - high_res_map references local_map, - working_map references working_map + -- Position oriented. + destination references destination, + world references 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 +-- 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. @@ -68,45 +87,6 @@ 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 -) - - -table local_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, - - -- 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 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 ) @@ -117,219 +97,174 @@ 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 -) - - -table estimated_position -( - -- Uses world coordinates, with increasing X,Y being rightward/upward. - x_meters float, - y_meters float, + -- 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, - -- 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, - - ---------------------------- + vertex_id uint32, + vertex references vertices + where latest_observation.vertex_id = vertices.id, ego references ego ) ------------------------------------------------------------------------ --- Event table(s) --- Pending actions should be put in an event table, as well as unexpected --- events. +-- Data tables (i.e., tables with multiple records) -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 - --- 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[], + vertices references vertices[], + 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 (VLR) to ego (necessary until one-way references are + -- supported) + ego references ego[] ) +------------------------------------------------ +-- Vertex related + -- 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. -table observations +-- 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 vertices ( + ------------------------------ + -- 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, + position references positions, + range_data references range_data, + motion references movements, - -- Actual position data. - actual_x_meters float, - actual_y_meters float, + graph_id uint32, + graph references graphs + where graphs.id = vertices.graph_id, - -- 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, + timestamp_sec double, ------------------------------ - -- 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. + vertex references vertices ) --- 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. + vertex references vertices ) --- 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, - - -- Observation corresponding to this sighting. - observation_id uint32, - 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 + -- This can be stored in a blob or in dedicated fields. + num_radials int32, + bearing_degs float[], + distance_meters float[], + + -- Constant reference established at record creation time. + vertex references vertices ) ------------------------------------------------------------------------- --- 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 vertices + using out_edges where edges.src_id = vertices.id, + + dest_id uint32, + dest references vertices + using in_edges where edges.dest_id = vertices.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 deleted file mode 100644 index 0987787..0000000 --- a/slam_sim/gaia/slam.ruleset +++ /dev/null @@ -1,273 +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 "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::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 -{ - //////////////////////////////////////////// - // Change of path state - - on_change(paths.state) - { - if ((state == slam_sim::PATH_STATE_STARTING) && (slam_sim::g_quit == 0)) - { - 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); - - // Create first observation. That will trigger the sequence - // of events moving toward the destination. - create_observation(paths); - } - } - - - on_update(paths.state) - { - if (paths.state == slam_sim::PATH_STATE_DONE) - { - 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."); - } - } - - - //////////////////////////////////////////// - // 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); - 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) - { - 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; - } - - // 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) - { - // Local map was updated. Now rebuild working map. - gaia_log::app().info("Area map built. Rebuilding local map."); - build_local_map(/local_map); - } - - - on_update(m: local_map) - { - // Local map was updated. Now rebuild working map. - gaia_log::app().info("Local map built. Rebuilding working map."); - build_working_map(/working_map); - } - - - 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 - - on_insert(c: collision_event) - { - // 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(); - } -} diff --git a/slam_sim/include/blob_cache.hpp b/slam_sim/include/blob_cache.hpp deleted file mode 100644 index 92d1fd5..0000000 --- a/slam_sim/include/blob_cache.hpp +++ /dev/null @@ -1,94 +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 - -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 -{ -public: - blob_t(uint32_t id, size_t size, uint32_t id_superseded_blob); - ~blob_t(); - - // Deallocate blob memory. - void clear(); - - // Reset blob content. - void reset(uint32_t id, size_t size, uint32_t id_superseded_blob); - -public: - uint32_t id; - - // Blob data information. - size_t size; - uint8_t* data; - - // Blob state. - blob_state_t state; - - // Blob that this blob supersedes. - // It will be deleted when this blob itself is superseded. - uint32_t id_superseded_blob; -}; - -class blob_cache_t -{ -public: - static blob_cache_t* get(); - -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); - - // 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; -}; diff --git a/slam_sim/include/constants.hpp b/slam_sim/include/constants.hpp new file mode 100644 index 0000000..fe1782b --- /dev/null +++ b/slam_sim/include/constants.hpp @@ -0,0 +1,43 @@ +#pragma once +#include +#include + +namespace slam_sim +{ + +// Degree/Radian conversions. +constexpr float c_rad_to_deg = (float) (180.0 / M_PI); +constexpr float c_deg_to_rad = (float) (M_PI / 180.0); + +// 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_navigation_map_node_width_meters = 0.25f; +// Map for output +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 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 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 travelled between each simulated step. +constexpr float c_step_meters = 0.8 * c_navigation_map_node_width_meters; + +// 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/globals.hpp b/slam_sim/include/globals.hpp new file mode 100644 index 0000000..92e7e53 --- /dev/null +++ b/slam_sim/include/globals.hpp @@ -0,0 +1,43 @@ +//////////////////////////////////////////////////////////////////////// +// 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 + +#include "constants.hpp" +#include "occupancy.hpp" + +namespace slam_sim +{ +//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; + +// 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; +extern float g_heading_degs; + +extern std::vector g_destinations; +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/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/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 dda2045..30ed59a 100644 --- a/slam_sim/include/occupancy.hpp +++ b/slam_sim/include/occupancy.hpp @@ -9,170 +9,215 @@ /*********************************************************************** -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. ***********************************************************************/ #include +#include #include #include "gaia_slam.h" +#include "map_types.hpp" #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. -// 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) - 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; - float landmarks; - - // Cost to traverse this node. This is computed as a function of the - // node's characteristics. - float traversal_cost; - - // Total cost to reach destination. - float distance; - - - void clear(); -}; - +//////////////////////////////////////////////////////////////////////// +// 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 { uint8_t occupied; uint8_t observed; uint8_t boundary; - uint8_t landmark; + uint8_t state; void clear(); }; +// 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 +#define PATH_NODE_FLAG_NEAR_IMPASSABLE 0x10 -struct grid_size_t +// 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 rows; - uint32_t cols; -}; + // Position of node w/in grid. + grid_coordinate_t pos; + // Index of node that's one-closer to destination (-1 for no parent) + 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; -struct grid_coord_t -{ - uint32_t x; - uint32_t y; -}; + float occupied; + float observed; + float boundary; + map_node_flags_t flags; -struct world_coordinate_t -{ - float x_meters; - float y_meters; -}; + // 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; -struct map_size_t -{ - float x_meters; - float y_meters; + void clear(); }; +// The grid (map) itself. 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(); + // 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); - // 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); + ~occupancy_grid_t(); - //void resize(float width_meters, float height_meters); + // 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); + 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); - // Apply sensor data to map from observation. - void apply_sensor_data(const gaia::slam::observations_t&); + 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&); 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, + occupancy_grid_t& parent_map); + 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: - 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, + // 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_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); 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_top_left; - world_coordinate_t m_bottom_right; + world_coordinate_t m_bottom_left; + // Note that positive coordiantes are rightward and upward. - std::vector m_grid; - std::vector m_grid_flags; + // 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; 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); -// + //////////////////////////////////////////////////////////////////// + // 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; + + // Add node to queue for future processing. + 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 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(); + + // 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/include/sensor_data.hpp b/slam_sim/include/sensor_data.hpp index 193be21..123b189 100644 --- a/slam_sim/include/sensor_data.hpp +++ b/slam_sim/include/sensor_data.hpp @@ -16,26 +16,26 @@ #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; struct sensor_data_t { - uint32_t num_radials; - // 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 5971470..5e03369 100644 --- a/slam_sim/include/slam_sim.hpp +++ b/slam_sim/include/slam_sim.hpp @@ -23,130 +23,83 @@ #pragma once #include "gaia_slam.h" + #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; -constexpr int32_t EXIT_AFTER_X_PATHS = 3; - -// Low-res map Alice has created of the surroundings. -extern occupancy_grid_t* g_area_map; +//////////////////////////////////////////////// +// 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. -constexpr float AREA_MAP_NODE_WIDTH_METERS = 0.25; -constexpr float LOCAL_MAP_NODE_WIDTH_METERS = AREA_MAP_NODE_WIDTH_METERS / 4.0; +// Determines if it's time to perform a graph optimization. +bool optimization_required(); -// 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; +// Stub function to graph optimization. Contents of function show how +// to iterate through a graph's vertices and edges. +void optimize_graph(gaia::slam::graphs_t&); +// Generates/updates path map. +void update_navigation_map(); -// 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; +// 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 reassess_destination(); -// How near to the destination we have to be to say "close enough". -constexpr double DESTINATION_RADIUS_METERS = 0.5; +// Updates the observed_area record to make sure it includes all observed +// areas plus the destination. +void update_world_area(); -// -constexpr double INTER_OBSERVATION_DIST_METERS = 0.5; +// Instructs hardware layer to move toward destination. +void move_toward_destination(); -// 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; +// Instructs bot to stop. An observation will be taken here. +void full_stop(); //////////////////////////////////////////////// -// Initialization +// Support API -// Params are Alice's starting point. -void seed_database(double initial_x_meters, double iniital_y_meters); +// Creates a record in the vertices table using the supplied sensor +// 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 +// the simulation more straightforward. +void generate_sensor_data(float pos_x_meters, float pos_y_meters, + sensor_data_t& data); -//////////////////////////////////////////////// -// Going places +// Generates and exports a map to disk. Filename will be based on ego's +// 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(); -// 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); +// 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(); -// Select position to go to. Default beahvior is to explore environment. -void select_destination(); -// Sets path.state to FIND_LANDMARK and selects a landmark to go to. -void select_landmark_destination(); +//////////////////////////////////////////////// +// Environmental analysis -// 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(); +// Calculates sensor view from a position in the environment. +void calculate_range_data(map_coord_t& coord, 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(); - -// Start a new path. This creates an initial observation and links it -// to a path. -void create_new_path(); +//////////////////////////////////////////////// +// Initialization -// Initialize a new path and assign it its first observation. -void init_path(const gaia::slam::observations_t&); +// Params are Alice's starting point. +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*); -//////////////////////////////////////////////// -// 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); - - -//////////////////////////////////////////////// -// 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&); - } // namespace slam_sim diff --git a/slam_sim/include/txn.hpp b/slam_sim/include/txn.hpp new file mode 100644 index 0000000..85d72bb --- /dev/null +++ b/slam_sim/include/txn.hpp @@ -0,0 +1,48 @@ +/////////////////////////////////////////////////////////////////////// +// 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. 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. +// +// 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 5fbed7b..21291ac 100644 --- a/slam_sim/src/Makefile +++ b/slam_sim/src/Makefile @@ -1,20 +1,54 @@ +include ../Makefile.inc -all: - mkdir -p ../build && cd ../build/ && cmake .. && make -j 4 +# 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) +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 +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 +OBJS = main.o $(CORE_OBJS) +EXT_OBJS = ../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) + +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) + +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 + #./$(TARGET) -m ../data/maze2.json -x -3.5 -y 1.5 + #./$(TARGET) -m ../data/maze.json -x -4.5 -y 1.25 + +drun: + 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) $(OPTIMIZATION_FLAG) $< + +refresh: clean all clean: - rm *.pgm + rm -f *.o $(TARGET) *.pnm + rm -rf logs + +clean_images: + rm -f *.pnm diff --git a/slam_sim/src/analyze.cpp b/slam_sim/src/analyze.cpp index 5896f28..35bc1b8 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'). // //////////////////////////////////////////////////////////////////////// @@ -29,8 +21,11 @@ #include +#include "constants.hpp" #include "json.hpp" +#include "globals.hpp" #include "line_segment.hpp" +#include "map_types.hpp" #include "sensor_data.hpp" namespace slam_sim @@ -40,15 +35,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,25 +52,22 @@ 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 +130,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..a284e1b 100644 --- a/slam_sim/src/globals.cpp +++ b/slam_sim/src/globals.cpp @@ -1,24 +1,33 @@ -//////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////// // 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 +// Globals for this namespace +#include + +#include "constants.hpp" #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; + +occupancy_grid_t g_navigation_map(c_navigation_map_node_width_meters); + +std::vector g_destinations; +uint32_t g_next_destination = 0; + +double g_now = 0.0; + +bool g_running = false; -// 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; +} // namespace slam_sim -} diff --git a/slam_sim/src/utils/line_segment.cpp b/slam_sim/src/line_segment.cpp similarity index 79% rename from slam_sim/src/utils/line_segment.cpp rename to slam_sim/src/line_segment.cpp index 696395a..ddb649a 100644 --- a/slam_sim/src/utils/line_segment.cpp +++ b/slam_sim/src/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 @@ -24,7 +28,7 @@ using namespace std; -namespace utils +namespace slam_sim { @@ -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,13 @@ 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); -//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); + double y3 = y; + double y4 = y + 10.0 * cos(D2R * theta); // 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); @@ -102,10 +101,10 @@ 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. + // 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,22 +115,24 @@ 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); 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. 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 utils +} // namespace slam_sim diff --git a/slam_sim/src/slam_sim.cpp b/slam_sim/src/main.cpp similarity index 60% rename from slam_sim/src/slam_sim.cpp rename to slam_sim/src/main.cpp index fc47424..dd8e548 100644 --- a/slam_sim/src/slam_sim.cpp +++ b/slam_sim/src/main.cpp @@ -20,63 +20,87 @@ #include "gaia_slam.h" +#include "globals.hpp" #include "slam_sim.hpp" -using namespace gaia::slam; +// To make video of output files, use, e.g.: +// ffmpeg -r 5 -i export_%03d.pnm out.mp4 -using std::this_thread::sleep_for; namespace slam_sim { -constexpr uint32_t c_rule_wait_millis = 100; +using std::this_thread::sleep_for; + +static constexpr uint32_t c_rule_wait_millis = 50; -static float g_initial_x_meters = -1.0; -static float g_initial_y_meters = -1.0; /** * Wait for simulation to complete. */ 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. while (g_quit == 0) { + if (g_running) + { + update_world_area(); + update_navigation_map(); + move_toward_destination(); + if (reassess_destination()) + { + reached_destinations++; + if (reached_destinations >= 3) + { + g_quit = 1; + } + } + if (ctr & 1) + { + build_export_map(); + } + if (ctr++ > 1200) + { + g_quit = 1; + } + } sleep_for(std::chrono::milliseconds(c_rule_wait_millis)); } } -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() +void clean_db() { - 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(); + gaia::db::commit_transaction(); } @@ -109,7 +133,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) { @@ -120,7 +144,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) { @@ -139,8 +163,8 @@ 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); + 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); } @@ -151,16 +175,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); - - // 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(); + slam_sim::seed_database(g_position.x_meters, g_position.y_meters); } } // namespace slam_sim; @@ -172,14 +187,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/navigation.cpp b/slam_sim/src/navigation.cpp index 4522ad9..a30d212 100644 --- a/slam_sim/src/navigation.cpp +++ b/slam_sim/src/navigation.cpp @@ -8,134 +8,287 @@ //////////////////////////////////////////////////////////////////////// // -// Primary API for rules relating to navigation and error estimation. +// Primary API for SLAM-related rules. // -// 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. +// Maps are generated from sensor data stored in the current graph. // //////////////////////////////////////////////////////////////////////// +#include #include +#include "gaia_slam.h" + +#include "constants.hpp" +#include "globals.hpp" +#include "occupancy.hpp" #include "slam_sim.hpp" +#include "txn.hpp" namespace slam_sim { using std::string; -using gaia::slam::paths_t; -using gaia::slam::observations_t; using gaia::slam::edges_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::ego_t; +using gaia::slam::graphs_t; +using gaia::slam::positions_t; +using gaia::slam::vertices_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::destination_t; +using gaia::slam::error_corrections_t; +using gaia::slam::observed_area_t; +using gaia::slam::destination_writer; -void calc_path_error(paths_t& path) +// 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 step. +bool optimization_required() { -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) - { - // 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()); + return true; +} + - if (!next.forward_edge()) +// 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. + //gaia_log::app().info("Stub function to optimize graph {}", graph.id()); + if (1 == 0) + { + // Here are some ways to itnerate through the data. + // By edges: + for (edges_t& e: edges_t::list()) + { + gaia_log::app().info("Edge connects vertices {} and {}", + e.src().id(), e.dest().id()); + } + // By vertices: + for (vertices_t& o: vertices_t::list()) { - break; + 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()); + } } - next = next.forward_edge().next(); } + // 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++; +} - // Updated error correction table. - for (error_correction_t& ec: error_correction_t::list()) - { - 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; +//////////////////////////////////////////////////////////////////////// +// Map building + +// Determines if sensor data extends beyond present area map and returns +// 'true' if so. +bool need_to_extend_map(positions_t& pos, observed_area_t& bounds) +{ + 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; } -void export_area_map() +void export_map_to_file() { static int32_t ctr = 0; - string fname("map_" + std::to_string(ctr) + ".pgm"); + char fname[256]; + sprintf(fname, "map_%03d.pnm", ctr++); gaia_log::app().info("Building map {}", fname); - g_area_map->export_as_pnm(fname); - ctr++; + g_navigation_map.export_as_pnm(fname); } -void build_area_map(area_map_t& am) +// Build an map for export. This is typically a higher resolution than +// the navigation map. +void build_export_map() { - g_area_map->clear(); - for (paths_t& p: paths_t::list()) + 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()) { - observations_t obs = p.first_observation(); - while (obs) + for (vertices_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(); + //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); +} + + +void update_navigation_map() +{ + txn_t txn; + txn.begin(); + destination_t dest = *(destination_t::list().begin()); + observed_area_t bounds = *(observed_area_t::list().begin()); + const world_coordinate_t bottom_left = { + .x_meters = bounds.left_meters(), + .y_meters = bounds.bottom_meters(), + }; + float width_meters = bounds.right_meters() - bounds.left_meters(); + float height_meters = bounds.top_meters() - bounds.bottom_meters(); + // Build map and apply sensor data. + g_navigation_map.reset(bottom_left, width_meters, height_meters); + for (graphs_t& g: graphs_t::list()) + { + for (vertices_t& v: g.vertices()) + { + g_navigation_map.apply_sensor_data(v); } } + // Build paths to destination. + world_coordinate_t pos = { + .x_meters = dest.x_meters(), + .y_meters = dest.y_meters() + }; + g_navigation_map.trace_routes(pos); + txn.commit(); +} - // TODO rebuild the area map - // In the meantime, just 'touch' the record by updating the - // change counter. - area_map_writer writer = am.writer(); - writer.change_counter = am.change_counter() + 1; - writer.update_row(); +//////////////////////////////////////////////////////////////////////// - export_area_map(); +void full_stop() +{ + // TODO Take observation. + // Stop moving. + g_running = false; + // TODO Need way to decide when to start running again. } -void build_local_map(local_map_t& lm) +bool reassess_destination() { - // 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(); + 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. + destination_t& dest = *(destination_t::list().begin()); + float dx = g_position.x_meters - dest.x_meters(); + float dy = g_position.y_meters - dest.y_meters(); + float dist_meters_2 = dx*dx + dy*dy; + const float close_enough_radius_2 = c_destination_radius_meters + * c_destination_radius_meters; + if (dist_meters_2 < close_enough_radius_2) + { + // Select next destination. + world_coordinate_t new_dest = g_destinations[g_next_destination++]; + 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; + } + 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; + rc = true; + } + txn.commit(); + return rc; } -void build_working_map(working_map_t& wm) + +// 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() { - // 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(); + 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 1b53dc0..40dd680 100644 --- a/slam_sim/src/occupancy.cpp +++ b/slam_sim/src/occupancy.cpp @@ -5,15 +5,28 @@ // 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 +#include #include #include +#include + +#include "constants.hpp" +#include "globals.hpp" #include "line_segment.hpp" #include "occupancy.hpp" #include "slam_sim.hpp" +#include "txn.hpp" namespace slam_sim { @@ -22,67 +35,79 @@ 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::destination_t;; +using gaia::slam::observed_area_t;; +using gaia::slam::positions_t; +using gaia::slam::range_data_t; +using gaia::slam::vertices_t; -using gaia::slam::landmark_sightings_t; -using gaia::slam::observations_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; -}; +// Construction and destruction -static sin_cos_t* s_sincos_lut = NULL; -static uint32_t s_sincos_lut_len = 0; +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); + // Physical size of map. + m_map_size.x_meters = width_meters; + m_map_size.y_meters = height_meters; + // Map placement. + m_bottom_left = bottom_left; +} -static void check_sincos_lut(uint32_t num_radials) +void occupancy_grid_t::allocate_grid(bool realloc_buffer) { - if (s_sincos_lut_len == 0) + uint32_t num_nodes = m_grid_size.rows * m_grid_size.cols; + if (realloc_buffer && (m_grid != NULL)) { - 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; iparent_idx = -1; + this->parent_idx.idx = c_invalid_grid_idx; this->direction_degs = 0.0f; this->occupied = 0.0f; this->observed = 0.0f; this->boundary = 0.0f; - this->landmarks = 0.0f; - this->traversal_cost = 0.0f; - this->distance = 0.0f; + this->flags.clear(); + + this->traversal_cost = 1.0f; + this->path_cost = 0.0f; } @@ -109,19 +135,21 @@ void map_node_flags_t::clear() this->occupied = 0; this->observed = 0; this->boundary = 0; - this->landmark = 0; + this->state = 0; } -void occupancy_grid_t::clear() +void occupancy_grid_t::initialize_grid() { - for (uint32_t i=0; i= m_grid_size.cols) { x_idx = m_grid_size.cols - 1; } - float bottom_inset_meters = y_meters - m_bottom_right.y_meters; - uint32_t y_idx = - (uint32_t) floor(bottom_inset_meters / m_map_size.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_node_size_meters); if (y_idx >= m_grid_size.rows) { y_idx = m_grid_size.rows - 1; } - return x_idx + y_idx * m_grid_size.cols; + grid_index_t idx = { .idx = x_idx + y_idx * m_grid_size.cols }; + return idx; } map_node_t& occupancy_grid_t::get_node(float x_meters, float y_meters) { - uint32_t idx = get_node_index(x_meters, y_meters); - return m_grid[idx]; + grid_index_t idx = get_node_index(x_meters, y_meters); + return m_grid[idx.idx]; +} + + +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) { - uint32_t idx = get_node_index(x_meters, y_meters); - return m_grid_flags[idx]; + grid_index_t idx = get_node_index(x_meters, y_meters); + return m_grid[idx.idx].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 -// 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(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); - 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]; - // Number of points on radial to examine. Sample at a fraction of + 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 // 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++) @@ -194,71 +245,116 @@ 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() { - 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; + } + } + // Mark impassable's neighbors as near impassable or close to + // impassable. + if (node.flags.state & PATH_NODE_FLAG_IMPASSABLE) + { + // 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] = { + -1, 0, 1, -1, 1, -1, 0, 1, + -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, + -3, -2, -1, 0, 1, 2, 3 + }; + 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, + -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<48; i++) + { + uint32_t nbr_x = (uint32_t) + ((int32_t) node.pos.x + x_delta[i]); + 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 |= + PATH_NODE_FLAG_ADJ_IMPASSABLE; + } + 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; + } + } + } + } } } -void occupancy_grid_t::apply_sensor_data(const observations_t& obs) +void occupancy_grid_t::apply_sensor_data(const vertices_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 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"); + // Draw position and boundaries. for (uint32_t y=0; y 0.0 ? 128 : 0; + g[idx] = node.occupied > 0.0 ? 255 : 0; } } - + // Indicate destination. + txn_t txn; + txn.begin(); + 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; + 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. - try + try { std::ofstream f(file_name, std::ofstream::binary); - f << "P6\n# Slam map\n" << m_grid_size.cols << " " + f << "P6\n# Slam map\n" << m_grid_size.cols << " " << m_grid_size.rows << "\n255\n"; for (uint32_t i=0; i> +* +* This is a support file for occupancy.cpp. +* +***********************************************************************/ + +#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) +// 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. 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, + /* 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); + 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)) + { + return; + } + ///////////////////////////////////////////////////////////////////// + // 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]; + if (child_node.flags.state & PATH_NODE_FLAG_IMPASSABLE) + { + return; + } + // Determine added weight for traversing this node. Have lower weight + // to nodes less frequently traversed to provide bias to take + // different routes. + 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. + assert(child_node.boundary == 0.0); + if (child_node.flags.state & PATH_NODE_FLAG_ADJ_IMPASSABLE) + { + weight += 100.0; + } + 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; + } + // Path tracing algorithm is deterministic and can follow vert or + // horiz paths 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; + 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) + { + // 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_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); +} + + +// 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 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); + 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)) + { + return; + } + ///////////////////////////////////////////////////////////////////// + // point is in the world -- check it + grid_index_t new_idx = { .idx = new_x + new_y * m_grid_size.cols }; + const map_node_t& child_node = m_grid[new_idx.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); + 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 = + PATH_NODE_FLAG_IMPASSABLE | PATH_NODE_FLAG_NEAR_IMPASSABLE; + if ((vert_child_node.flags.state & pass_mask) && + (horiz_child_node.flags.state & pass_mask)) + { + // no direct 4-connected path. nothing to do here + return; + } + add_node_to_stack(root_idx, offset, 1.41f); +} + +// 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 +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. + 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_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); +} + + +// 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_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 + // 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; y 0) && (y < m_grid_size.rows-1)) + { + // In this row, anchor first and last columns only + 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); + 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 + { + // This is a top or bottom row. Anchor all nodes in this row. + for (uint32_t x=0; x -#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/support.cpp b/slam_sim/src/support.cpp new file mode 100644 index 0000000..b17ecf2 --- /dev/null +++ b/slam_sim/src/support.cpp @@ -0,0 +1,266 @@ +//////////////////////////////////////////////////////////////////////// +// 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 "gaia_slam.h" + +#include "constants.hpp" +#include "globals.hpp" +#include "occupancy.hpp" +#include "slam_sim.hpp" +#include "txn.hpp" + + +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::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; +using gaia::slam::observed_area_t; +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; + + +// Updates the observed_area record to make sure it includes all observed +// areas plus the destination. +void update_world_area() +{ + txn_t txn; + txn.begin(); + ego_t ego = *(ego_t::list().begin()); + + observed_area_t area = ego.world(); + destination_t dest = ego.destination(); + + bool change = false; + // Left and right bounds. + 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) + { + 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 = 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); + 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(); + } + 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) +{ + txn_t txn; + txn.begin(); + 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(); + float prev_x_meters, prev_y_meters, prev_heading_degs; + 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(); + } + 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 + 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( + c_num_range_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_world_area(); + + // 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(); + + txn.commit(); +} + + +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. + txn_t txn; + txn.begin(); + + //////////////////////////////////////////// + // 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 + ); + + world_coordinate_t new_dest = g_destinations[g_next_destination++]; + assert(g_destinations.size() > 1); + + // 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 + // 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 + right, // right_meters + top, // top_meters + bottom // bottom_meters + ); + + gaia_id_t destination_id = destination_t::insert_row( + new_dest.x_meters, // x_meters + new_dest.y_meters, // y_meters + 0.0 // departure_time_sec + ); + gaia_log::app().info("Initial destination is {},{}", new_dest.x_meters, + new_dest.y_meters); + //////////////////////////////////////////// + // Relationships + ego.destination().connect(destination_id); + ego.world().connect(world_id); + ego.latest_observation().connect(latest_observation_id); + + //////////////////////////////////////////// + // All done + txn.commit(); +} + +} // namespace slam_sim + diff --git a/slam_sim/tests/test_analyze.cpp b/slam_sim/src/test_analyze.cpp similarity index 53% rename from slam_sim/tests/test_analyze.cpp rename to slam_sim/src/test_analyze.cpp index 666ae69..a2bfc0c 100644 --- a/slam_sim/tests/test_analyze.cpp +++ b/slam_sim/src/test_analyze.cpp @@ -15,28 +15,23 @@ #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 utils; +using namespace slam_sim; 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); + 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; @@ -66,44 +62,35 @@ int main(int argc, char** argv) load_world_map(WORLD_MAP_FILE); std::string response; sensor_data_t data; - perform_sensor_sweep(-3.0, -4.4, data); - - // wall distances + 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(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); + errs += check_radial(44, 2.9, 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); + coord.heading_degs = 135.0; + calculate_range_data(coord, data); + errs += check_radial(22, -1.0, 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++; - } - } + // 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) diff --git a/slam_sim/tests/test_line_segment.cpp b/slam_sim/src/test_line_segment.cpp similarity index 87% rename from slam_sim/tests/test_line_segment.cpp rename to slam_sim/src/test_line_segment.cpp index 9a2e69c..6821f06 100644 --- a/slam_sim/tests/test_line_segment.cpp +++ b/slam_sim/src/test_line_segment.cpp @@ -13,7 +13,7 @@ using namespace std; -using utils::line_segment_t; +using slam_sim::line_segment_t; uint32_t check_distance(double len, double expected) { @@ -30,8 +30,8 @@ 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); + // 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); @@ -39,8 +39,8 @@ int main() 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); + // 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); @@ -48,7 +48,7 @@ int main() 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); + 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); 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 + diff --git a/slam_sim/src/utils/Makefile b/slam_sim/src/utils/Makefile deleted file mode 100644 index 5c0a329..0000000 --- a/slam_sim/src/utils/Makefile +++ /dev/null @@ -1,25 +0,0 @@ -include ../../Makefile.inc - -TEST_TARGETS = test_line_segment - -OBJS = line_segment.o landmark_description.o blob_cache.o - - -all: $(OBJS) - ar rcs $(LIB_SLAM_UTILS) *.o - -test: $(TEST_TARGETS) - -test_line_segment: line_segment.cpp - $(CXX) $(CPPFLAGS) line_segment.cpp -o test_line_segment -DTEST_LINE_SEGMENT - - -%.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 c88a08f..0000000 --- a/slam_sim/src/utils/README_blob_cache.md +++ /dev/null @@ -1,36 +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. -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. -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 = blob_factory_t.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 3da31b9..0000000 --- a/slam_sim/src/utils/blob_cache.cpp +++ /dev/null @@ -1,116 +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" - -blob_cache_t blob_cache_t::s_blob_cache; - -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(); - - this->id = id; - this->size = size; - this->id_superseded_blob = id_superseded_blob; - - this->state = blob_state_t::initialized; - - 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) -{ - 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!"); - - std::unique_lock unique_lock(m_lock); - - // Lookup superseded blob. - blob_t* superseded_blob_ptr = nullptr; - if (id_superseded_blob != c_invalid_blob_id) - { - superseded_blob_ptr = get_blob(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); - 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)); - } - - // 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) - { - 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; -} - -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!"); - - std::shared_lock shared_lock(m_lock); - - auto iterator = m_blob_map.find(id); - return (iterator == m_blob_map.end()) ? nullptr : iterator->second; -} 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/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; -}