diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..ad06a32a --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,15 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: noetic, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/docs/core_functions/ros_launch.rst b/docs/core_functions/ros_launch.rst index 81c6eb0d..9dcde888 100644 --- a/docs/core_functions/ros_launch.rst +++ b/docs/core_functions/ros_launch.rst @@ -25,7 +25,8 @@ Here are the full list of parameters with the default values step_size:=0.005 \ show_viz:=true \ viz_pub_rate:=30.0 \ - use_rviz:=false + use_rviz:=false \ + simplify_map:=2 * **world_path**: path to world.yaml * **update_rate**: the real time rate to run the simulation loop in Hz @@ -33,4 +34,5 @@ Here are the full list of parameters with the default values * **show_viz**: show visualization, pops the flatland_viz window and publishes visualization messages, either true or false * **viz_pub_rate**: rate to publish visualization in Hz, works only when show_viz=true -* **use_rviz**: works only when show_viz=true, set this to disable flatland_viz popup \ No newline at end of file +* **use_rviz**: works only when show_viz=true, set this to disable flatland_viz popup +* **simplify_map**: Simpify map during vector tracing: 0=None (default), 1=moderately, 2=significantly \ No newline at end of file diff --git a/docs/core_functions/yaml_preprocessor.rst b/docs/core_functions/yaml_preprocessor.rst index c125bec7..09910f5b 100644 --- a/docs/core_functions/yaml_preprocessor.rst +++ b/docs/core_functions/yaml_preprocessor.rst @@ -9,9 +9,60 @@ Yaml Preprocessor Flatland Server has a lua preprocessor for YAML with simple bindings for the environment variables and rosparam. The intent is to be able to build parametric models that are defined by dimensions and flags found in either environment variables or rosparam, in a similar way to xacro+urdf. Because this is parsed at model load time, any roslaunch rosparam loading will have completed and those parameters will be available. +Including additional YAML files +------------------------------- +The YAML preprocessor allows for including of other YAML files using the `$include` keyword. When this string is encountered, the following filename is parsed as a YAML file and substituted for the `$include`-string. The file path bay be absolute or relative. Relative file paths are relative to the YAML file currently being processed. + +.. code-block:: yaml + + # project/param/parent.yaml + foo: 123 + bar: $include child.yaml # Looks in current directory (project/param/) for relative filenames + baz: $include /absolute/path/to/project/param/child.yaml # or an absolute path can be specified + + # project/param/child.yaml + a: 1 + b: 2 + + # result of YAML preprocessing parent.yaml: + foo: 123 + bar: + a: 1 + b: 2 + baz: + a: 1 + b: 2 + +There is also a special form of `$include`, `$[include]`, that can be used to populate a sequence with an arbitrary number of items. Each individual document in the specified YAML file (separated by `---` as is standard for multi-document YAML files) becomes its own element in the sequence. For example: + +.. code-block:: yaml + + # parent.yaml + foo: + - first + - $[include] child.yaml + - last + + #child.yaml + one + --- + a: foo + b: baz + --- + three + + # result of YAML preprocessing of parent.yaml: + foo: + - first + - one + - a: foo + b: baz + - three + - last + body/joint/plugin enabled flag ------------------------------ -Model bodies, plugins and joints now have a new flag `enabled` which can be set to true or false either directly in the yaml, or based on more complex logic from a lua `$eval` string that returns "true" or "false". Disabled bodies, plugins and joints are skipped during yaml loading, and as a result are never instantiated. From Flatland's perspective `enabled: false` causes the affected body/plugin/joint to be deleted. +Model bodies, plugins and joints now have a new flag `enabled` which can be set to true or false either directly in the yaml, or based on more complex logic from a lua `$eval` string that returns "true" or "false". Disabled bodies, plugins and joints are skipped during yaml loading, and as a result are never instantiated. From Flatland's perspective `enabled: false` causes the affected body/plugin/joint to be deleted. bindings for env and param ------------------------------- diff --git a/docs/design/2026-03-24-box2d-v3-migration/agent.spec.md b/docs/design/2026-03-24-box2d-v3-migration/agent.spec.md new file mode 100644 index 00000000..810582b7 --- /dev/null +++ b/docs/design/2026-03-24-box2d-v3-migration/agent.spec.md @@ -0,0 +1,184 @@ +# Box2D v3 Migration — Agent-Facing Spec + +> **For Claude:** REQUIRED SUB-SKILL: Use executing-plans to implement this plan task-by-task. + +## Repo & Branch + +- **Repo:** `C:\Users\Rapyuta Robotics\Desktop\flatland` (rapyuta-robotics/flatland fork) +- **Branch to create:** `feature/box2d-v3` +- **Box2D v3 version:** `v3.1.0` from `https://github.com/erincatto/box2d` +- **enkiTS version:** `v2.10` from `https://github.com/dougbinks/enkiTS` + +## Build System + +- ROS1 Catkin workspace +- `flatland_server/CMakeLists.txt` — main package, builds `flatland_server` node and library +- `flatland_plugins/CMakeLists.txt` — builds plugin shared libraries +- Box2D 2.3.2 is vendored at `flatland_server/thirdparty/Box2D/` and added via `add_subdirectory` as target `flatland_Box2D` +- ThreadPool header at `flatland_plugins/thirdparty/ThreadPool.h` — used only in `laser.h`, to be removed + +## File Inventory (all files that need changes) + +### Layer 0 — CMake +- `flatland_server/CMakeLists.txt` — remove Box2D 2.x, add Box2D v3 + enkiTS via FetchContent + +### Layer 1 — Core Lifecycle +- `flatland_server/include/flatland_server/world.h` — `b2World*`→`b2WorldId`, remove `b2ContactListener` base +- `flatland_server/src/world.cpp` — `new b2World`→`b2CreateWorld`, `Step`→`b2World_Step` +- `flatland_server/include/flatland_server/body.h` — `b2Body*`→`b2BodyId` +- `flatland_server/src/body.cpp` — `CreateBody`→`b2CreateBody`, `SetUserData`, `DestroyBody` +- `flatland_server/include/flatland_server/joint.h` — `b2Joint*`→`b2JointId` +- `flatland_server/src/joint.cpp` — `CreateJoint`→`b2CreateRevoluteJoint`/`b2CreateWeldJoint`, `frequencyHz`→`stiffness` +- `flatland_server/include/flatland_server/layer.h` — `b2World*`→`b2WorldId` +- `flatland_server/src/layer.cpp` — body/fixture creation, `b2EdgeShape::Set`→`b2Segment` +- `flatland_server/src/model_body.cpp` — `b2FixtureDef`→`b2ShapeDef`, shape structs +- `flatland_server/include/flatland_server/model_body.h` — `b2FixtureDef`→`b2ShapeDef` +- `flatland_server/include/flatland_server/entity.h` — may store `b2World*`, update to `b2WorldId` + +### Layer 2 — Contact System +- `flatland_server/src/world.cpp` — poll `b2World_GetContactEvents()` after Step +- `flatland_server/include/flatland_server/flatland_plugin.h` — callback signatures `(b2Contact*)` → `(b2ShapeId, b2ShapeId)` +- `flatland_server/src/plugin_manager.cpp` — dispatch with new signatures +- `flatland_server/include/flatland_server/plugin_manager.h` — same +- `flatland_server/include/flatland_server/model_plugin.h` — same +- `flatland_server/src/model_plugin.cpp` — `fixture->GetBody()->GetUserData()` → `b2Shape_GetBody` + `b2Body_GetUserData` +- `flatland_plugins/src/bumper.cpp` — rewrite BeginContact/EndContact/PostSolve +- `flatland_plugins/include/flatland_plugins/bumper.h` — signature updates +- `flatland_plugins/src/bool_sensor.cpp` — rewrite BeginContact/EndContact + +### Layer 3 — Ray Casts +- `flatland_plugins/include/flatland_plugins/laser.h` — delete `LaserCallback` class, `ThreadPool pool_` +- `flatland_plugins/src/laser.cpp` — replace parallel pool with sequential `b2World_CastRay` + free function +- `flatland_plugins/include/flatland_plugins/world_modifier.h` — delete `RayTrace` class +- `flatland_plugins/src/world_modifier.cpp` — replace with free function + +### Layer 4 — Remaining Plugins +- `flatland_plugins/src/diff_drive.cpp` — body accessor API updates +- `flatland_plugins/src/tricycle_drive.cpp` — body accessors + revolute joint accessor +- `flatland_plugins/src/gps.cpp` — `GetTransform`→`b2Body_GetTransform` +- `flatland_plugins/src/model_tf_publisher.cpp` — same +- `flatland_plugins/src/tween.cpp` — `SetTransform`→`b2Body_SetTransform` +- `flatland_server/src/debug_visualization.cpp` — shape type enum + field accessor updates +- `flatland_plugins/src/world_random_wall.cpp` — `b2MulT`→`b2InvTransformPoint` + +### Layer 5 — Threading +- `flatland_server/src/world.cpp` — add enkiTS scheduler, wire `b2WorldDef` task callbacks +- `flatland_server/include/flatland_server/world.h` — add `enkiTaskScheduler*` member + +## Critical v2→v3 API Mapping + +### World +| v2 | v3 | +|---|---| +| `new b2World(gravity)` | `b2CreateWorld(&b2DefaultWorldDef())` | +| `delete physics_world_` | `b2DestroyWorld(worldId_)` | +| `physics_world_->Step(dt, vIter, pIter)` | `b2World_Step(worldId_, dt, subSteps)` (note: `position_iterations` deprecated — use `subSteps=4`) | +| `physics_world_->SetContactListener(this)` | removed — poll events instead | +| `physics_world_->RayCast(&cb, p1, p2)` | `b2World_CastRay(worldId_, origin, translation, filter, fcn, ctx)` | + +### Body +| v2 | v3 | +|---|---| +| `b2Body* body = world->CreateBody(&def)` | `b2BodyId bodyId = b2CreateBody(worldId, &def)` | +| `body->SetUserData(ptr)` | `b2Body_SetUserData(bodyId, ptr)` | +| `body->GetUserData()` | `b2Body_GetUserData(bodyId)` | +| `world->DestroyBody(body)` | `b2DestroyBody(bodyId)` | +| `body->CreateFixture(&fixtureDef)` | `b2CreatePolygonShape(bodyId, &shapeDef, &polygon)` etc. | +| `body->GetPosition()` | `b2Body_GetPosition(bodyId)` | +| `body->GetAngle()` | `b2Body_GetRotation(bodyId)` → `.angle` or `b2Rot_GetAngle(rot)` | +| `body->GetTransform()` | `b2Body_GetTransform(bodyId)` | +| `body->SetTransform(pos, angle)` | `b2Body_SetTransform(bodyId, pos, b2MakeRot(angle))` | +| `body->GetLinearVelocity()` | `b2Body_GetLinearVelocity(bodyId)` | +| `body->SetLinearVelocity(v)` | `b2Body_SetLinearVelocity(bodyId, v)` | +| `body->GetAngularVelocity()` | `b2Body_GetAngularVelocity(bodyId)` | +| `body->SetAngularVelocity(w)` | `b2Body_SetAngularVelocity(bodyId, w)` | +| `body->GetWorldCenter()` | `b2Body_GetWorldCenterOfMass(bodyId)` | +| `body->GetWorldVector(v)` | `b2Body_GetWorldVector(bodyId, v)` | +| `body->GetWorldPoint(p)` | `b2Body_GetWorldPoint(bodyId, p)` | +| `body->GetLinearVelocityFromLocalPoint(p)` | `b2Body_GetLinearVelocityFromLocalPoint(bodyId, p)` (check v3 API — may need manual: vel + cross(omega, r)) | + +### Fixture → Shape +| v2 | v3 | +|---|---| +| `b2FixtureDef` | `b2ShapeDef` | +| `fixtureDef.filter.categoryBits` | `shapeDef.filter.categoryBits` | +| `fixtureDef.isSensor` | `shapeDef.isSensor` | +| `fixtureDef.friction` | `shapeDef.friction` | +| `fixtureDef.restitution` | `shapeDef.restitution` | +| `b2CircleShape shape; shape.m_p = center; shape.m_radius = r;` | `b2Circle circle = {center, r};` then `b2CreateCircleShape(bodyId, &shapeDef, &circle)` | +| `b2PolygonShape shape; shape.Set(pts, n);` | `b2Polygon polygon = b2MakePolygon(&hull, 0);` then `b2CreatePolygonShape(bodyId, &shapeDef, &polygon)` | +| `b2EdgeShape edge; edge.Set(v1, v2);` | `b2Segment seg = {v1, v2};` then `b2CreateSegmentShape(bodyId, &shapeDef, &seg)` | +| `fixture->GetFilterData().categoryBits` | `b2Shape_GetFilter(shapeId).categoryBits` | +| `fixture->IsSensor()` | `b2Shape_IsSensor(shapeId)` | +| `fixture->GetBody()` | `b2Shape_GetBody(shapeId)` | +| `fixture->GetNext()` | `b2Body_GetShapes(bodyId, shapes, capacity)` (batch query) | +| `fixture->GetShape()->GetType()` | `b2Shape_GetType(shapeId)` | + +### Joint +| v2 | v3 | +|---|---| +| `b2RevoluteJointDef def; def.Initialize(bA,bB,anchor);` | `b2RevoluteJointDef def = b2DefaultRevoluteJointDef();` then set `bodyIdA/B`, `localAnchorA/B` | +| `world->CreateJoint(&def)` → `b2Joint*` | `b2CreateRevoluteJoint(worldId, &def)` → `b2JointId` | +| `b2WeldJointDef.frequencyHz` | `b2WeldJointDef.angularHertz` (or `linearHertz`) | +| `b2WeldJointDef.dampingRatio` | `b2WeldJointDef.angularDampingRatio` | +| `dynamic_cast(joint)` | Direct `b2JointId` typed calls `b2RevoluteJoint_GetAngle(jId)` | +| `joint->SetMotorSpeed(s)` | `b2RevoluteJoint_SetMotorSpeed(jId, s)` | +| `joint->GetBodyA()` | `b2Joint_GetBodyA(jId)` | +| `joint->GetAnchorA()` | `b2Joint_GetConstraintForce(jId, dt)` etc. / `b2Body_GetWorldPoint(b2Joint_GetBodyA(jId), localAnchor)` | + +### Contact Events (v3 only — replaces b2ContactListener) +```cpp +// After b2World_Step(): +b2ContactEvents events = b2World_GetContactEvents(worldId_); +for (int i = 0; i < events.beginCount; i++) { + b2ContactBeginTouchEvent* e = &events.beginEvents[i]; + // e->shapeIdA, e->shapeIdB + // To get Body*: static_cast(b2Body_GetUserData(b2Shape_GetBody(e->shapeIdA))) +} +for (int i = 0; i < events.endCount; i++) { + b2ContactEndTouchEvent* e = &events.endEvents[i]; +} +// Hit events replace PostSolve: +b2ContactHitEvent* hitEvents = events.hitEvents; // events.hitCount +``` + +### Ray Cast (v3) +```cpp +// Free function signature: +float LaserRayCastFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, + float fraction, void* context) { + auto* ctx = static_cast(context); + // check filter: b2Shape_GetFilter(shapeId).categoryBits + if (b2Shape_IsSensor(shapeId)) return -1.0f; // ignore sensors + ctx->hit = true; + ctx->fraction = fraction; + ctx->point = point; + ctx->normal = normal; + return fraction; // return fraction to find closest hit +} +// Dispatch: +b2QueryFilter filter = b2DefaultQueryFilter(); +filter.maskBits = laser_layers_mask; +b2World_CastRay(worldId_, origin, translation, filter, LaserRayCastFcn, &context); +``` + +### Transform +| v2 | v3 | +|---|---| +| `b2Transform t; t.p.x; t.p.y; t.q.c; t.q.s;` | `b2Transform t; t.p.x; t.p.y; t.q.c; t.q.s;` (same struct layout) | +| `b2MulT(transform, point)` | `b2InvTransformPoint(transform, point)` | + +### Math +| v2 | v3 | +|---|---| +| `b2Vec2` | `b2Vec2` (unchanged) | +| `b2_staticBody`, `b2_dynamicBody`, `b2_kinematicBody` | `b2_staticBody`, `b2_dynamicBody`, `b2_kinematicBody` (same names in v3) | +| `b2_maxPolygonVertices` | `B2_MAX_POLYGON_VERTICES` (renamed) | + +## Test Files +- `flatland_server/test/` — gtest suite, run with `catkin run_tests flatland_server` +- `flatland_plugins/test/` — gtest suite, run with `catkin run_tests flatland_plugins` + +## Includes +- v2: `#include ` +- v3: `#include ` diff --git a/docs/design/2026-03-24-box2d-v3-migration/plan.md b/docs/design/2026-03-24-box2d-v3-migration/plan.md new file mode 100644 index 00000000..afc76f51 --- /dev/null +++ b/docs/design/2026-03-24-box2d-v3-migration/plan.md @@ -0,0 +1,843 @@ +# Box2D v3 Migration Implementation Plan + +> **For Claude:** REQUIRED SUB-SKILL: Use executing-plans to implement this plan task-by-task. + +**Goal:** Replace Box2D 2.3.2 with Box2D v3.1.0 in the rapyuta-robotics/flatland fork, wire in enkiTS for multi-core solver, preserving all existing behavior. + +**Architecture:** Incremental layer-by-layer on `feature/box2d-v3` branch. Each layer is independently compilable. Six layers: CMake → Core lifecycle → Contact system → Ray casts → Plugins → Threading. + +**Tech Stack:** C++17, ROS1 Catkin, Box2D v3.1.0 (C17 API), enkiTS v2.10, CMake FetchContent. + +**Reference:** Read `docs/design/2026-03-24-box2d-v3-migration/agent.spec.md` for complete API mapping before starting any layer. + +--- + +## Task 0: Create feature branch + +**Files:** none + +**Step 1: Create and checkout branch** +```bash +cd C:\Users\Rapyuta Robotics\Desktop\flatland +git checkout -b feature/box2d-v3 +``` + +**Step 2: Verify** +```bash +git branch --show-current +``` +Expected: `feature/box2d-v3` + +**Step 3: Commit** +```bash +git commit --allow-empty -m "chore: start box2d-v3 migration branch" +``` + +--- + +## Task 1: Layer 0 — CMake — Swap Box2D 2.x for v3 + enkiTS + +**Files:** +- Modify: `flatland_server/CMakeLists.txt` + +**Step 1: Read the current CMakeLists.txt** + +Read `flatland_server/CMakeLists.txt` fully to understand current structure before editing. + +**Step 2: Replace Box2D vendored build with FetchContent for v3 + enkiTS** + +Find the block `add_subdirectory("thirdparty/Box2D")` and the `flatland_Box2D` target references. Replace with: + +```cmake +include(FetchContent) + +FetchContent_Declare( + box2d + GIT_REPOSITORY https://github.com/erincatto/box2d.git + GIT_TAG v3.1.0 +) +FetchContent_MakeAvailable(box2d) + +FetchContent_Declare( + enkiTS + GIT_REPOSITORY https://github.com/dougbinks/enkiTS.git + GIT_TAG v2.10 +) +FetchContent_MakeAvailable(enkiTS) +``` + +Replace all `flatland_Box2D` → `box2d` in `target_link_libraries`. +Add `enkiTS` to `target_link_libraries` for `flatland_server`. + +**Step 3: Remove thirdparty Box2D directory reference** + +Remove or comment out the `add_subdirectory("thirdparty/Box2D")` line. + +**Step 4: Attempt build — it WILL fail (all #includes are still Box2D v2)** +```bash +cd +catkin build flatland_server --no-deps 2>&1 | head -50 +``` +Expected: compile errors about `Box2D/Box2D.h` not found — this is correct at this stage. CMake configure step should succeed. + +**Step 5: Commit the CMake change** +```bash +git add flatland_server/CMakeLists.txt +git commit -m "build(layer0): replace vendored Box2D 2.x with Box2D v3.1.0 + enkiTS via FetchContent" +``` + +--- + +## Task 2: Layer 1a — Update all #includes + +**Files:** +- All `.cpp` and `.h` files in `flatland_server/` and `flatland_plugins/` that `#include ` + +**Step 1: Find all files with the old include** +```bash +grep -r "#include " flatland_server/ flatland_plugins/ --include="*.h" --include="*.cpp" -l +``` + +**Step 2: Batch replace in every file found** + +Replace `#include ` → `#include ` in all matching files. + +Also remove `#include ` from `flatland_plugins/include/flatland_plugins/laser.h` (ThreadPool is being deleted in Layer 3). + +**Step 3: Commit** +```bash +git add -A +git commit -m "refactor(layer1): update Box2D include paths to v3" +``` + +--- + +## Task 3: Layer 1b — Migrate world.h + entity.h headers + +**Files:** +- Modify: `flatland_server/include/flatland_server/world.h` +- Modify: `flatland_server/include/flatland_server/entity.h` + +**Step 1: Read both files in full** + +**Step 2: Update world.h** +- Remove `public b2ContactListener` from `World` class declaration +- Change `b2World* physics_world_` → `b2WorldId worldId_` +- Change `b2Vec2 gravity_` — `b2Vec2` is still valid in v3, keep +- Remove declarations: `BeginContact`, `EndContact`, `PreSolve`, `PostSolve` +- Add member: `enkiTaskScheduler* taskScheduler_ = nullptr;` + +**Step 3: Update entity.h** +- If `b2World*` is stored, change to `b2WorldId` + +**Step 4: Attempt header compile check** +```bash +catkin build flatland_server --no-deps 2>&1 | grep "world.h\|entity.h" | head -20 +``` + +**Step 5: Commit** +```bash +git add flatland_server/include/flatland_server/world.h flatland_server/include/flatland_server/entity.h +git commit -m "refactor(layer1): update world.h and entity.h to Box2D v3 types" +``` + +--- + +## Task 4: Layer 1c — Migrate body.h + body.cpp + +**Files:** +- Modify: `flatland_server/include/flatland_server/body.h` +- Modify: `flatland_server/src/body.cpp` + +**Step 1: Read both files in full** + +**Step 2: Update body.h** +- `b2Body* physics_body_` → `b2BodyId physics_body_` +- Any `b2World*` parameter → `b2WorldId` +- Any `b2BodyType` field — check if still used (v3 keeps `b2BodyType` enum with same names) + +**Step 3: Update body.cpp** using the API mapping in agent.spec.md: +- `world->CreateBody(&body_def)` → `b2CreateBody(worldId, &body_def)` + - `b2BodyDef` fields are mostly the same in v3; verify `type`, `position`, `angle`, `linearVelocity`, `angularVelocity`, `linearDamping`, `angularDamping` +- `physics_body_->SetUserData(this)` → `b2Body_SetUserData(physics_body_, this)` +- `physics_body_->GetWorld()->DestroyBody(physics_body_)` → `b2DestroyBody(physics_body_)` +- `GetPosition()` → `b2Body_GetPosition(physics_body_)` +- `GetAngle()` → `b2Rot_GetAngle(b2Body_GetRotation(physics_body_))` +- `GetType()` → `b2Body_GetType(physics_body_)` +- `GetLinearDamping()` / `GetAngularDamping()` → `b2Body_GetLinearDamping` / `b2Body_GetAngularDamping` +- `GetFixtureList()` — replace with `b2Body_GetShapeCount` + `b2Body_GetShapes` + +**Step 4: Attempt compile** +```bash +catkin build flatland_server --no-deps 2>&1 | grep "body\." | head -30 +``` + +**Step 5: Commit** +```bash +git add flatland_server/include/flatland_server/body.h flatland_server/src/body.cpp +git commit -m "refactor(layer1): migrate body.h/cpp to Box2D v3 ID API" +``` + +--- + +## Task 5: Layer 1d — Migrate joint.h + joint.cpp + +**Files:** +- Modify: `flatland_server/include/flatland_server/joint.h` +- Modify: `flatland_server/src/joint.cpp` + +**Step 1: Read both files** + +**Step 2: Update joint.h** +- `b2Joint* physics_joint_` → `b2JointId physics_joint_` +- `b2JointDef` stored? Replace with appropriate typed def or remove + +**Step 3: Update joint.cpp** +- `world->CreateJoint(&def)` for revolute → `b2CreateRevoluteJoint(worldId, &revoluteDef)` +- `world->CreateJoint(&def)` for weld → `b2CreateWeldJoint(worldId, &weldDef)` +- `physics_joint_->SetUserData(this)` → `b2Joint_SetUserData(physics_joint_, this)` (check v3 API — may be `b2RevoluteJoint_SetUserData` or body user data) +- `b2WeldJointDef.frequencyHz` → `angularHertz` +- `b2WeldJointDef.dampingRatio` → `angularDampingRatio` +- `world->DestroyJoint(physics_joint_)` → `b2DestroyJoint(physics_joint_)` +- `GetBodyA()` / `GetBodyB()` → `b2Joint_GetBodyA(jId)` / `b2Joint_GetBodyB(jId)` +- `GetAnchorA()` / `GetAnchorB()` — compute via `b2Body_GetWorldPoint` + +**Step 4: Compile check** +```bash +catkin build flatland_server --no-deps 2>&1 | grep "joint\." | head -30 +``` + +**Step 5: Commit** +```bash +git add flatland_server/include/flatland_server/joint.h flatland_server/src/joint.cpp +git commit -m "refactor(layer1): migrate joint.h/cpp to Box2D v3 ID API" +``` + +--- + +## Task 6: Layer 1e — Migrate layer.h + layer.cpp + +**Files:** +- Modify: `flatland_server/include/flatland_server/layer.h` +- Modify: `flatland_server/src/layer.cpp` + +**Step 1: Read both files** + +**Step 2: Update layer.h** +- `b2World*` → `b2WorldId` + +**Step 3: Update layer.cpp** +- Static body creation: `b2BodyDef` → use `b2DefaultBodyDef()`, `type = b2_staticBody` +- `b2EdgeShape edge; edge.Set(v1, v2); b2FixtureDef fd; body->CreateFixture(&fd)` → + ```cpp + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.filter = layerFilter; + b2Segment seg = {v1, v2}; + b2CreateSegmentShape(bodyId, &shapeDef, &seg); + ``` +- Update all `b2FixtureDef` usages similarly + +**Step 4: Compile and fix** +```bash +catkin build flatland_server --no-deps 2>&1 | grep "layer\." | head -30 +``` + +**Step 5: Commit** +```bash +git add flatland_server/include/flatland_server/layer.h flatland_server/src/layer.cpp +git commit -m "refactor(layer1): migrate layer.h/cpp to Box2D v3 shape API" +``` + +--- + +## Task 7: Layer 1f — Migrate model_body.h + model_body.cpp + +**Files:** +- Modify: `flatland_server/include/flatland_server/model_body.h` +- Modify: `flatland_server/src/model_body.cpp` + +**Step 1: Read both files** + +**Step 2: Update model_body.h** +- `b2FixtureDef` stored → `b2ShapeDef` + +**Step 3: Update model_body.cpp** +- Circle: `b2CircleShape shape; shape.m_p.Set(x,y); shape.m_radius = r;` → + ```cpp + b2Circle circle; + circle.center = {x, y}; + circle.radius = r; + b2CreateCircleShape(bodyId, &shapeDef, &circle); + ``` +- Polygon: `b2PolygonShape shape; shape.Set(pts, n);` → + ```cpp + b2Hull hull = b2ComputeHull(pts, n); + b2Polygon polygon = b2MakePolygon(&hull, 0.0f); + b2CreatePolygonShape(bodyId, &shapeDef, &polygon); + ``` +- `b2_maxPolygonVertices` → `B2_MAX_POLYGON_VERTICES` + +**Step 4: Compile and fix** +```bash +catkin build flatland_server --no-deps 2>&1 | grep "model_body\." | head -30 +``` + +**Step 5: Commit** +```bash +git add flatland_server/include/flatland_server/model_body.h flatland_server/src/model_body.cpp +git commit -m "refactor(layer1): migrate model_body to Box2D v3 shape API" +``` + +--- + +## Task 8: Layer 1g — Migrate world.cpp core lifecycle + +**Files:** +- Modify: `flatland_server/src/world.cpp` + +**Step 1: Read world.cpp in full** + +**Step 2: Replace world creation** +```cpp +// Remove: +physics_world_ = new b2World(gravity_); +physics_world_->SetContactListener(this); + +// Add: +b2WorldDef worldDef = b2DefaultWorldDef(); +worldDef.gravity = gravity_; +worldId_ = b2CreateWorld(&worldDef); +``` + +**Step 3: Replace world destruction** +```cpp +// Remove: +physics_world_->SetContactListener(nullptr); +delete physics_world_; + +// Add: +b2DestroyWorld(worldId_); +``` + +**Step 4: Replace Step call** +```cpp +// Remove: +physics_world_->Step(timekeeper_.GetStepSize(), + physics_velocity_iterations_, + physics_position_iterations_); + +// Add (subSteps replaces position_iterations — use 4 as default): +b2World_Step(worldId_, + static_cast(timekeeper_.GetStepSize()), + physics_velocity_iterations_); // velocity iterations still used +``` + +**Step 5: Remove BeginContact/EndContact/PreSolve/PostSolve implementations** + +These 4 method bodies are deleted from world.cpp. The contact dispatch moves to Layer 2. + +**Step 6: Compile Layer 1 fully** +```bash +catkin build flatland_server --no-deps 2>&1 | tail -30 +``` +Expected: `flatland_server` builds cleanly (plugins still broken — that's fine). + +**Step 7: Run existing server tests** +```bash +catkin run_tests flatland_server --no-deps +catkin_test_results build/flatland_server +``` +Expected: some failures due to contact/fixture access in tests — note which, fix in subsequent tasks. + +**Step 8: Commit** +```bash +git add flatland_server/src/world.cpp +git commit -m "refactor(layer1): migrate world.cpp core lifecycle to Box2D v3" +``` + +--- + +## Task 9: Layer 2a — Update plugin contact callback signatures + +**Files:** +- Modify: `flatland_server/include/flatland_server/flatland_plugin.h` +- Modify: `flatland_server/include/flatland_server/plugin_manager.h` +- Modify: `flatland_server/include/flatland_server/model_plugin.h` + +**Step 1: Read all three files** + +**Step 2: Update flatland_plugin.h** + +Change contact callback virtual method signatures: +```cpp +// Remove: +virtual void BeginContact(b2Contact* contact) {} +virtual void EndContact(b2Contact* contact) {} +virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) {} +virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) {} + +// Add: +virtual void BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) {} +virtual void EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) {} +// PreSolve removed — no equivalent in v3 +// PostSolve replaced by hit events: +virtual void OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, float approachSpeed) {} +``` + +**Step 3: Update plugin_manager.h** — same signature changes for dispatch methods. + +**Step 4: Update model_plugin.h** — same signature changes. + +**Step 5: Commit headers** +```bash +git add flatland_server/include/flatland_server/flatland_plugin.h \ + flatland_server/include/flatland_server/plugin_manager.h \ + flatland_server/include/flatland_server/model_plugin.h +git commit -m "refactor(layer2): update contact callback signatures for Box2D v3" +``` + +--- + +## Task 10: Layer 2b — Migrate plugin_manager.cpp + model_plugin.cpp + +**Files:** +- Modify: `flatland_server/src/plugin_manager.cpp` +- Modify: `flatland_server/src/model_plugin.cpp` + +**Step 1: Read both files** + +**Step 2: Update plugin_manager.cpp** + +Dispatch methods now take `(b2ShapeId, b2ShapeId)` — update body of each method to call plugins with new signature. + +**Step 3: Update model_plugin.cpp** + +The fixture→body→Body* pattern: +```cpp +// Remove: +Body* bodyA = static_cast(contact->GetFixtureA()->GetBody()->GetUserData()); + +// Add: +Body* bodyA = static_cast(b2Body_GetUserData(b2Shape_GetBody(shapeIdA))); +``` + +**Step 4: Compile check** +```bash +catkin build flatland_server --no-deps 2>&1 | tail -30 +``` + +**Step 5: Commit** +```bash +git add flatland_server/src/plugin_manager.cpp flatland_server/src/model_plugin.cpp +git commit -m "refactor(layer2): migrate plugin_manager and model_plugin to v3 contact API" +``` + +--- + +## Task 11: Layer 2c — Implement contact event polling in world.cpp + +**Files:** +- Modify: `flatland_server/src/world.cpp` + +**Step 1: Read world.cpp Update() / step function** + +**Step 2: Add event polling after b2World_Step** + +```cpp +void World::Update(Timekeeper& timekeeper) { + // ... existing pre-step logic ... + + b2World_Step(worldId_, static_cast(timekeeper.GetStepSize()), + physics_velocity_iterations_); + + // --- Contact event dispatch (replaces b2ContactListener callbacks) --- + b2ContactEvents events = b2World_GetContactEvents(worldId_); + + for (int i = 0; i < events.beginCount; i++) { + const b2ContactBeginTouchEvent& e = events.beginEvents[i]; + plugin_manager_.BeginContact(e.shapeIdA, e.shapeIdB); + } + for (int i = 0; i < events.endCount; i++) { + const b2ContactEndTouchEvent& e = events.endEvents[i]; + plugin_manager_.EndContact(e.shapeIdA, e.shapeIdB); + } + for (int i = 0; i < events.hitCount; i++) { + const b2ContactHitEvent& e = events.hitEvents[i]; + plugin_manager_.OnContactHit(e.shapeIdA, e.shapeIdB, + e.point, e.normal, e.approachSpeed); + } + // --- End contact dispatch --- + + // ... existing post-step logic ... +} +``` + +**Step 3: Compile and run tests** +```bash +catkin build flatland_server --no-deps +catkin run_tests flatland_server --no-deps +catkin_test_results build/flatland_server +``` +Expected: contact-related tests pass. + +**Step 4: Commit** +```bash +git add flatland_server/src/world.cpp +git commit -m "feat(layer2): implement Box2D v3 contact event polling in World::Update" +``` + +--- + +## Task 12: Layer 2d — Migrate bumper.cpp + bool_sensor.cpp + +**Files:** +- Modify: `flatland_plugins/src/bumper.cpp` +- Modify: `flatland_plugins/include/flatland_plugins/bumper.h` +- Modify: `flatland_plugins/src/bool_sensor.cpp` + +**Step 1: Read all three files in full** + +**Step 2: Update bumper.h + bumper.cpp** +- Change `BeginContact(b2Contact*)` → `BeginContact(b2ShapeId, b2ShapeId)` +- Change `EndContact(b2Contact*)` → `EndContact(b2ShapeId, b2ShapeId)` +- Change `PostSolve(b2Contact*, b2ContactImpulse*)` → `OnContactHit(..., float approachSpeed)` +- Replace `contact->GetFixtureA()` → use `shapeIdA` directly +- Replace `fixture->GetBody()->GetUserData()` → `b2Body_GetUserData(b2Shape_GetBody(shapeIdA))` +- Replace `b2WorldManifold` usage → use `point` and `normal` from `OnContactHit` parameters +- Replace impulse access (`impulse->normalImpulses[0]`) → use `approachSpeed` from hit event + +**Step 3: Update bool_sensor.cpp** +- Same signature updates for `BeginContact` / `EndContact` +- `contact->GetFixtureA()->IsSensor()` → `b2Shape_IsSensor(shapeIdA)` +- `contact->GetFixtureA()` → `shapeIdA` + +**Step 4: Build plugins** +```bash +catkin build flatland_plugins --no-deps 2>&1 | grep "bumper\|bool_sensor" | head -30 +``` + +**Step 5: Run plugin tests** +```bash +catkin run_tests flatland_plugins --no-deps +catkin_test_results build/flatland_plugins +``` + +**Step 6: Commit** +```bash +git add flatland_plugins/src/bumper.cpp flatland_plugins/include/flatland_plugins/bumper.h \ + flatland_plugins/src/bool_sensor.cpp +git commit -m "refactor(layer2): migrate bumper and bool_sensor to Box2D v3 contact events" +``` + +--- + +## Task 13: Layer 3a — Migrate laser.cpp (ray casts) + +**Files:** +- Modify: `flatland_plugins/include/flatland_plugins/laser.h` +- Modify: `flatland_plugins/src/laser.cpp` + +**Step 1: Read both files in full** + +**Step 2: Update laser.h** +- Delete `LaserCallback` nested class entirely +- Delete `ThreadPool pool_` member +- Delete `#include ` +- Add struct for ray cast context: + ```cpp + struct LaserRayContext { + bool hit = false; + float fraction = 1.0f; + b2Vec2 point = {0, 0}; + b2Vec2 normal = {0, 0}; + }; + ``` + +**Step 3: Update laser.cpp** + +Delete the thread pool parallel loop. Replace it with a sequential loop: + +```cpp +// Old parallel pattern (DELETE): +std::vector> results(num_beams); +for (int i = 0; i < num_beams; i++) { + results[i] = pool_.enqueue([i, this, ...] { + LaserCallback cb(...); + GetModel()->GetPhysicsWorld()->RayCast(&cb, p1, p2); + ... + }); +} + +// New sequential pattern: +for (int i = 0; i < num_beams; i++) { + float angle = angle_min + i * angle_increment; + b2Vec2 origin = ...; // compute from body transform + b2Vec2 end = ...; // origin + range * direction + + LaserRayContext ctx; + b2QueryFilter filter = b2DefaultQueryFilter(); + filter.maskBits = laser_layers_bits_; + b2Vec2 translation = {end.x - origin.x, end.y - origin.y}; + b2World_CastRay(worldId_, origin, translation, filter, LaserRayCastFcn, &ctx); + + scan_msg.ranges[i] = ctx.hit ? ctx.fraction * range_max_ : range_max_; + // intensities if needed +} +``` + +Add free function (outside class, before the method): +```cpp +static float LaserRayCastFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, + float fraction, void* context) { + // Filter sensors + if (b2Shape_IsSensor(shapeId)) return -1.0f; + auto* ctx = static_cast(context); + ctx->hit = true; + ctx->fraction = fraction; + ctx->point = point; + ctx->normal = normal; + return fraction; // return fraction to continue looking for closer hits +} +``` + +**Step 4: Update how worldId is accessed** + +The `Laser` plugin needs access to `b2WorldId`. Currently it calls `GetModel()->GetPhysicsWorld()` returning `b2World*`. Update `model.h` / `world.h` to expose `b2WorldId GetPhysicsWorldId()` accessor. + +**Step 5: Build and test laser** +```bash +catkin build flatland_plugins --no-deps 2>&1 | grep "laser" | head -30 +catkin run_tests flatland_plugins --no-deps -- --gtest_filter="*Laser*" +catkin_test_results build/flatland_plugins +``` + +**Step 6: Commit** +```bash +git add flatland_plugins/include/flatland_plugins/laser.h flatland_plugins/src/laser.cpp +git commit -m "refactor(layer3): replace parallel ThreadPool ray casts with sequential b2World_CastRay" +``` + +--- + +## Task 14: Layer 3b — Migrate world_modifier.cpp (ray casts) + +**Files:** +- Modify: `flatland_plugins/include/flatland_plugins/world_modifier.h` +- Modify: `flatland_plugins/src/world_modifier.cpp` + +**Step 1: Read both files** + +**Step 2: Delete `RayTrace` callback class from world_modifier.h** + +Add context struct if needed. + +**Step 3: Replace RayCast call in world_modifier.cpp with free function pattern** + +Same pattern as laser — create a `WorldModifierRayCastFcn` free function, use `b2World_CastRay`. + +**Step 4: Build and commit** +```bash +catkin build flatland_plugins --no-deps 2>&1 | grep "world_modifier" | head -20 +git add flatland_plugins/include/flatland_plugins/world_modifier.h \ + flatland_plugins/src/world_modifier.cpp +git commit -m "refactor(layer3): migrate world_modifier ray cast to Box2D v3" +``` + +--- + +## Task 15: Layer 4 — Migrate remaining plugins + +**Files:** +- Modify: `flatland_plugins/src/diff_drive.cpp` +- Modify: `flatland_plugins/src/tricycle_drive.cpp` +- Modify: `flatland_plugins/src/gps.cpp` +- Modify: `flatland_plugins/src/model_tf_publisher.cpp` +- Modify: `flatland_plugins/src/tween.cpp` +- Modify: `flatland_plugins/src/world_random_wall.cpp` +- Modify: `flatland_server/src/debug_visualization.cpp` + +**Step 1: Read all files** (read them in parallel) + +**Step 2: Migrate diff_drive.cpp** +Using agent.spec.md body accessor table: +- `GetPosition()` → `b2Body_GetPosition` +- `GetAngle()` → `b2Rot_GetAngle(b2Body_GetRotation(id))` +- `GetWorldVector(v)` → `b2Body_GetWorldVector(id, v)` +- `GetWorldCenter()` → `b2Body_GetWorldCenterOfMass(id)` +- `SetLinearVelocity` → `b2Body_SetLinearVelocity` +- `SetAngularVelocity` → `b2Body_SetAngularVelocity` +- `GetLinearVelocityFromLocalPoint` → manual: `vel + cross(omega, localPt)` or check v3 API + +**Step 3: Migrate tricycle_drive.cpp** +Same as diff_drive plus: +- `dynamic_cast(joint)` → `b2RevoluteJoint_GetAngle(jId)` directly +- `rev_joint->SetMotorSpeed(s)` → `b2RevoluteJoint_SetMotorSpeed(jId, s)` + +**Step 4: Migrate gps.cpp, model_tf_publisher.cpp, tween.cpp** +- `GetTransform()` → `b2Body_GetTransform(id)` — returns `b2Transform` (same struct layout as v2) +- `t.q.c`, `t.q.s` — `b2Rot` fields are `c` and `s` in v3 (same) +- `SetTransform(pos, angle)` → `b2Body_SetTransform(id, pos, b2MakeRot(angle))` + +**Step 5: Migrate world_random_wall.cpp** +- `b2MulT(transform, point)` → `b2InvTransformPoint(transform, point)` +- Shape/fixture access: `fixture->GetShape()` → use `b2ShapeId` based accessor +- `fixture->GetNext()` → iterate via `b2Body_GetShapes` + +**Step 6: Migrate debug_visualization.cpp** +- `b2Shape::e_circle` → `b2_circleShape` +- `b2Shape::e_polygon` → `b2_polygonShape` +- `b2Shape::e_edge` → `b2_segmentShape` +- `shape->m_count`, `shape->m_vertices[i]` → `b2Shape_GetPolygon(shapeId)` returns `b2Polygon` struct +- `shape->m_p`, `shape->m_radius` → `b2Shape_GetCircle(shapeId)` returns `b2Circle` struct +- `fixture->GetNext()` → batch `b2Body_GetShapes` +- `body->GetFixtureList()` → `b2Body_GetShapes` + +**Step 7: Build all plugins** +```bash +catkin build flatland_plugins --no-deps 2>&1 | tail -20 +``` +Expected: clean build. + +**Step 8: Run all plugin tests** +```bash +catkin run_tests flatland_plugins --no-deps +catkin_test_results build/flatland_plugins +``` +Expected: 0 failures. + +**Step 9: Commit** +```bash +git add flatland_plugins/src/diff_drive.cpp flatland_plugins/src/tricycle_drive.cpp \ + flatland_plugins/src/gps.cpp flatland_plugins/src/model_tf_publisher.cpp \ + flatland_plugins/src/tween.cpp flatland_plugins/src/world_random_wall.cpp \ + flatland_server/src/debug_visualization.cpp +git commit -m "refactor(layer4): migrate all remaining plugins to Box2D v3 API" +``` + +--- + +## Task 16: Layer 5 — Wire enkiTS for multi-core solver + +**Files:** +- Modify: `flatland_server/include/flatland_server/world.h` +- Modify: `flatland_server/src/world.cpp` +- Modify: `flatland_server/CMakeLists.txt` (verify enkiTS is linked) + +**Step 1: Read world.h and world.cpp** + +**Step 2: Add enkiTS header include and member in world.h** +```cpp +#include + +class World : ... { + ... +private: + enki::TaskScheduler taskScheduler_; // owns the thread pool + ... +}; +``` + +**Step 3: Wire enkiTS task callbacks in world.cpp CreateWorld** + +```cpp +// Define task callbacks (file scope static functions): +static void* EnkiEnqueueTask(b2TaskCallback* box2dTask, int itemCount, + int minRange, void* box2dTaskContext, void* userContext) { + auto* scheduler = static_cast(userContext); + auto* taskSet = new enki::TaskSet(itemCount, + [box2dTask, box2dTaskContext](enki::TaskSetPartition range, uint32_t threadNum) { + box2dTask(range.start, range.end, threadNum, box2dTaskContext); + }); + taskSet->m_MinRange = minRange; + scheduler->AddTaskSetToPipe(taskSet); + return taskSet; +} + +static void EnkiFinishTask(void* taskPtr, void* userContext) { + auto* scheduler = static_cast(userContext); + auto* taskSet = static_cast(taskPtr); + scheduler->WaitforTask(taskSet); + delete taskSet; +} + +// In World constructor / MakeWorld(): +taskScheduler_.Initialize(); // uses hardware_concurrency() threads + +b2WorldDef worldDef = b2DefaultWorldDef(); +worldDef.gravity = gravity_; +worldDef.workerCount = taskScheduler_.GetNumTaskThreads(); +worldDef.enqueueTask = EnkiEnqueueTask; +worldDef.finishTask = EnkiFinishTask; +worldDef.userTaskContext = &taskScheduler_; +worldId_ = b2CreateWorld(&worldDef); +``` + +**Step 4: Destroy scheduler after world** +In destructor / Shutdown: +```cpp +b2DestroyWorld(worldId_); +taskScheduler_.WaitforAllAndShutdown(); +``` + +**Step 5: Build** +```bash +catkin build flatland_server flatland_plugins --no-deps 2>&1 | tail -10 +``` +Expected: clean build. + +**Step 6: Run full test suite** +```bash +catkin run_tests flatland_server flatland_plugins +catkin_test_results build/flatland_server build/flatland_plugins +``` +Expected: 0 failures. + +**Step 7: Verify multi-core utilization** + +Start the sim and monitor: +```bash +roslaunch sootballs_bringup simulation.launch & +# In another terminal: +top -d 1 | grep flatland +# or: +htop # observe CPU columns +``` +Expected: CPU utilization spread across multiple cores during `b2World_Step`. + +**Step 8: Final commit** +```bash +git add flatland_server/include/flatland_server/world.h flatland_server/src/world.cpp +git commit -m "feat(layer5): wire enkiTS multi-core task scheduler into Box2D v3 world" +``` + +--- + +## Task 17: Final verification + +**Step 1: Full build from clean** +```bash +catkin clean flatland_server flatland_plugins +catkin build flatland_server flatland_plugins +``` + +**Step 2: All tests pass** +```bash +catkin run_tests flatland_server flatland_plugins +catkin_test_results build/flatland_server build/flatland_plugins +``` +Expected output: `Summary: X tests, 0 errors, 0 failures` + +**Step 3: Integration smoke test with rr_sootballs** +```bash +cd C:\Users\Rapyuta Robotics\Desktop\rr_sootballs +SIM_TYPE=flatland tilt up -- --site demo --num_robots 5 +``` +Observe: simulation runs, robots navigate, laser scans publish correctly. + +**Step 4: Push branch** +```bash +cd C:\Users\Rapyuta Robotics\Desktop\flatland +git log --oneline feature/box2d-v3 | head -20 +git push origin feature/box2d-v3 +``` diff --git a/docs/design/2026-03-24-box2d-v3-migration/spec.md b/docs/design/2026-03-24-box2d-v3-migration/spec.md new file mode 100644 index 00000000..631861a7 --- /dev/null +++ b/docs/design/2026-03-24-box2d-v3-migration/spec.md @@ -0,0 +1,79 @@ +# Box2D v3 Migration + Multi-Core Physics + +**Date:** 2026-03-24 +**Repo:** rapyuta-robotics/flatland +**Branch:** `feature/box2d-v3` + +## Goal + +Replace the vendored Box2D 2.3.2 physics engine with Box2D v3.x and wire in enkiTS as the task scheduler, enabling the Flatland simulator to use all available CPU cores for physics simulation. All existing ROS interfaces, launch parameters, and simulation behavior are preserved. + +## Background + +Flatland is a 2D robot simulator used in rr_sootballs warehouse simulation. It currently runs on a single CPU core because Box2D 2.x's `b2World::Step()` is single-threaded by design. Box2D v3 (released August 2024) is a complete rewrite in C17 with native multi-threading via a pluggable task-system interface. By wiring enkiTS into the `b2WorldDef` task callbacks, the solver, broad-phase, and island solving all run across all available cores with no changes to simulation logic. + +## Non-Goals + +- No ROS interface changes (topics, services, parameters) +- No changes in the rr_sootballs repo +- No new simulation features +- No GPU physics + +## Architecture + +### Threading Model + +**Option A (chosen):** Box2D v3 internal solver threading via enkiTS. Box2D v3 exposes `b2WorldDef.enqueueTask` / `b2WorldDef.finishTask` callbacks. We provide an enkiTS scheduler instance and hook these callbacks. Box2D partitions its internal work graph (island solving, broad-phase) and schedules parallel tasks through the provided callbacks. Ray casts remain single-threaded — the existing `ThreadPool`-based ray cast parallelism in `laser.cpp` must be removed as `b2World_CastRay` is not thread-safe to call concurrently from external threads in v3. + +### Migration Strategy + +**Approach 2 (chosen):** Incremental layer-by-layer on feature branch. Six layers, each independently compilable and testable: + +``` +Layer 0: CMake — swap Box2D 2.3.2 for v3 + add enkiTS +Layer 1: Core lifecycle — world, body, joint, layer (pointers → IDs) +Layer 2: Contact system — callbacks → event polling +Layer 3: Ray casts — virtual classes → free functions +Layer 4: Remaining plugins +Layer 5: Threading — wire enkiTS into b2WorldDef +``` + +## Key API Differences + +### Pointer → ID System +Box2D v3 replaces all `b2Body*`, `b2Fixture*`, `b2Joint*` raw pointers with opaque integer ID structs (`b2BodyId`, `b2ShapeId`, `b2JointId`). All code that stores, passes, or dereferences these pointers must be updated. + +### Contact System +Box2D v3 removes the `b2ContactListener` virtual callback interface entirely. Contacts are collected internally and exposed via `b2World_GetContactEvents()` which must be polled after each `b2World_Step()`. The dispatch chain `World → PluginManager → plugin callbacks` is preserved but the entry point changes from virtual overrides to a polling loop in `World::Update()`. + +### Ray Cast System +Box2D v3 replaces `b2RayCastCallback` virtual classes with a C-style function pointer `b2CastResultFcn`. The `LaserCallback` and `RayTrace` virtual classes are deleted and replaced with free functions. The parallel `ThreadPool` in `laser.cpp` is removed. + +### User Data +`b2Body::SetUserData(void*)` → `b2Body_SetUserData(b2BodyId, void*)`. All fixture→body→`Body*` casts in `bumper.cpp` and `model_plugin.cpp` must be updated accordingly. + +### enkiTS Threading +```cpp +b2WorldDef worldDef = b2DefaultWorldDef(); +worldDef.workerCount = std::thread::hardware_concurrency(); +worldDef.enqueueTask = [](b2TaskCallback* task, int itemCount, int minRange, + void* taskContext, void* userContext) -> void* { + auto* ts = static_cast(userContext); + auto* set = new enkiTaskSet; // pool-managed in production + enkiInitTaskSet(ts, set, task, taskContext, itemCount, minRange); + enkiAddTaskSet(ts, set); + return set; +}; +worldDef.finishTask = [](void* taskPtr, void* userContext) { + auto* ts = static_cast(userContext); + enkiWaitForTaskSet(ts, static_cast(taskPtr)); +}; +worldDef.userTaskContext = enkiTaskSchedulerPtr_; +``` + +## Acceptance Criteria + +1. All existing tests in `flatland_server/test/` and `flatland_plugins/test/` pass with 0 failures +2. `catkin_test_results` clean +3. Running `rr_sootballs` with `SIM_TYPE=flatland` produces no behaviour regressions +4. CPU monitoring during `b2World_Step` shows >1 core utilization on multi-core machine diff --git a/docs/included_plugins/diff_drive.rst b/docs/included_plugins/diff_drive.rst index a4c9b6b0..9a6205fe 100644 --- a/docs/included_plugins/diff_drive.rst +++ b/docs/included_plugins/diff_drive.rst @@ -21,10 +21,10 @@ velocities and odometries are w.r.t. the robot origin plugins: # required, specify DiffDrive type to load the plugin - - type: DiffDrive + - type: DiffDrive # required, name of the plugin - name: turtlebot_drive + name: turtlebot_drive # required, body of a model to set velocities and obtain odometry body: base @@ -50,29 +50,34 @@ velocities and odometries are w.r.t. the robot origin # optional, defaults to "twist", the topic to publish noisy local frame velocity # that simulates encoder readings twist_pub: twist - + # optional, defaults to true, enables the advertising and publishing of both # ground truth and noisy odometry enable_odom_pub: true - + + # optional, defaults to true, enables the publishing of the TF between + # 'base_link' (or link specified in "body") and 'odom' links. + # Disable if your code has its own TF publisher for these two links. + enable_odom_tf_pub: true + # optional, defaults to true, enables the advertising and publishing of noisy local # frame velocity enable_twist_pub: true - # optional, defaults to [0, 0, 0], corresponds to noise on [x, y, yaw], + # optional, defaults to [0, 0, 0], corresponds to noise on [x, y, yaw], # the variances of gaussian noise to apply to the pose components of the # odometry message odom_pose_noise: [0, 0, 0] - # optional, defaults to [0, 0, 0], corresponds to noise on + # optional, defaults to [0, 0, 0], corresponds to noise on # [x velocity, y velocity, yaw rate], the variances of gaussian noise to # apply to the twist components of the odometry message odom_twist_noise: [0, 0, 0] - # optional, defaults to the diagonal [x, y, yaw] components replaced by - # odom_pose_noise with all other values equals zero, must have length of 36, - # represents a 6x6 covariance matrix for x, y, z, roll, pitch, yaw. - # This does not involve in any of the noise calculation, it is simply + # optional, defaults to the diagonal [x, y, yaw] components replaced by + # odom_pose_noise with all other values equals zero, must have length of 36, + # represents a 6x6 covariance matrix for x, y, z, roll, pitch, yaw. + # This does not involve in any of the noise calculation, it is simply # the output values of odometry pose covariance odom_pose_covariance: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -81,10 +86,10 @@ velocities and odometries are w.r.t. the robot origin 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - # optional, defaults to the diagonal [x velocity, y velocity, yaw rate] + # optional, defaults to the diagonal [x velocity, y velocity, yaw rate] # components replaced by odom_twist_noise with all other values equals zero, - # must have length of 36, represents a 6x6 covariance matrix for rates x, - # y, z, roll, pitch, yaw. This does not involve in any of the noise + # must have length of 36, represents a 6x6 covariance matrix for rates x, + # y, z, roll, pitch, yaw. This does not involve in any of the noise # calculation, it is simply the output values of odometry twist covariance odom_twist_covariance: [0, 0, 0, 0, 0, 0 0, 0, 0, 0, 0, 0 @@ -105,4 +110,6 @@ velocities and odometries are w.r.t. the robot origin linear_dynamics: acceleration_limit: 0.0 # max acceleration (away from 0), in m/s/s; 0.0 means "no limit" deceleration_limit: 0.0 # max deceleration (towards 0), in m/s/s; 0.0 means "no limit"; left blank, will default to acceleration_limit value - velocity_limit: 0.0 # max absolute velocity in m/s; 0.0 means "no limit" \ No newline at end of file + velocity_limit: 0.0 # max absolute velocity in m/s; 0.0 means "no limit" + + ground_truth_frame_id: map # The name of the ground truth origin TF frame \ No newline at end of file diff --git a/docs/included_plugins/imu.rst b/docs/included_plugins/imu.rst new file mode 100644 index 00000000..7810db8c --- /dev/null +++ b/docs/included_plugins/imu.rst @@ -0,0 +1,55 @@ +.. image:: ../_static/flatland_logo2.png + :width: 250px + :align: right + :target: ../_static/flatland_logo2.png + + +IMU +========== +This plugin provides a simple simulation of an IMU (three-axis accelerometer and three-axis gyroscope). + +* The reference frame of the IMU is x-forward, y-left, z-up. Because Flatland is 2D, only the x and y axes are populated with accelerations only the z axis is populated with an angular rate, and the orientation has components only in the z and w fields. + +* The measurements are modelled in a simple manner; they are simply the ground truth corrupted with zero-mean Gaussian noise. + +* Publishes a `sensor_msgs/Imu `_ message with the simulated measurements + +.. code-block:: yaml + + plugins: + + # required, specify Imu type to load the plugin + - type: Imu + + # required, name of the plugin + name: turtlebot_imu + + # required, body of a model to set location of simulated IMU + body: base + + # optional, defaults to true, whether to publish the IMU data + enable_imu_pub: true + + # optional, defaults to imu, TF frame name of the IMU + imu_frame_id: imu + + # optional, defaults to imu/filtered, the topic on which the noisy imu data is published + imu_pub: imu/filtered + + # optional, defaults to imu/ground_truth, the topic on which the true (no noise added) imu data is published + ground_truth_pub: imu/ground_truth + + # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the orientation . Note that because the simulation is 2D, only the third value (i.e., yaw noise) is used. + orientation_noise: [0.0, 0.0, 0.0] + + # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the angular velocity. Note that because the simulation is 2D, only the third value (i.e., yaw rate noise) is used. + angular_velocity_noise: [0.0, 0.0, 0.0] + + # optional, defaults to [0.0, 0.0, 0.0], the diagonal of the covariance matrix of the zero-mean noise added to the linear acceleration. Note that because the simulation is 2D, only the first two values (i.e., x and y acceleration noise) are used. + linear_acceleration_noise: [0.0, 0.0, 0.0] + + # optional, defaults to inifinity (i.e., as fast as the simulation updates), rate to publish IMU measurements, in Hz + update_rate: 40 + + # optional, defaults to true, whether to publish TF + broadcast_tf: true diff --git a/docs/included_plugins/laser.rst b/docs/included_plugins/laser.rst index d94f6e72..d600653d 100644 --- a/docs/included_plugins/laser.rst +++ b/docs/included_plugins/laser.rst @@ -34,6 +34,9 @@ messages. # optional, default to true, whether to publish TF broadcast_tf: true + # optional, default to false, if true compute/publish even if no subscribers (for benchmarking) + always_publish: false + # optional, default to name of this plugin, the TF frame id to publish TF with # only used when broadcast_tf=true frame: laser_back @@ -46,6 +49,9 @@ messages. # required, w.r.t to the coordinate system, scan from min angle to max angle # at steps of specified increments + # Scan direction defaults to counter-clockwise but clockwise rotations can be + # simulated by providing a decrementing angle configuration. + # e.g. min: 2, max: -2, increment: -0.1 (clockwise) angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} # optional, default to inf (as fast as possible), rate to publish laser scan messages @@ -55,6 +61,13 @@ messages. # lasers only detects objects in the specified layers layers: ["all"] + # optional, invert the mounting orientation of the lidar (default: false) + # This will invert the laser's body->laser frame TF (roll=PI) + # And sweep the lidar scan across a field mirrored across its mounted axis + # (as if you physically mounted the lidar upside down) + upside_down: false + + # another example - type: Laser name: laser_front @@ -64,4 +77,4 @@ messages. layers: ["layer_1", "layer_2", "layer_3"] update_rate: 100 noise_std_dev: 0.01 - \ No newline at end of file + diff --git a/docs/included_plugins/tricycle_drive.rst b/docs/included_plugins/tricycle_drive.rst index 371ccb1e..f4b58e8c 100644 --- a/docs/included_plugins/tricycle_drive.rst +++ b/docs/included_plugins/tricycle_drive.rst @@ -147,4 +147,6 @@ has no effect on the actual motion of the robot. # optional, defaults to 0.0 which means no limit # sets the steering angular acceleration limit (absolute, rad/s^2) # the steering angular acceleration will not exceed this absolute limit - max_steer_acceleration: 0.0 \ No newline at end of file + max_steer_acceleration: 0.0 + + ground_truth_frame_id: map # The name of the ground truth origin TF frame \ No newline at end of file diff --git a/flatland/CHANGELOG.rst b/flatland/CHANGELOG.rst index 239f9826..049136dc 100644 --- a/flatland/CHANGELOG.rst +++ b/flatland/CHANGELOG.rst @@ -4,6 +4,14 @@ Change Log This project adheres to `Semantic Versioning `_. +1.4.1 (2023-11-24) +------------------ +* CMake version required bump to fix ros build farm warning + +1.4.0 (2023-11-22) +------------------ +* Version Bump + 1.1.0 ------------------ * Added Lua based parametric model yaml files diff --git a/flatland/CMakeLists.txt b/flatland/CMakeLists.txt index 6c3375a4..437fe7fe 100644 --- a/flatland/CMakeLists.txt +++ b/flatland/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(flatland) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/flatland/LICENSE b/flatland/LICENSE new file mode 100644 index 00000000..32802b81 --- /dev/null +++ b/flatland/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, Avidbots Corp. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/flatland/package.xml b/flatland/package.xml index 890a6ad5..8ee31ccd 100644 --- a/flatland/package.xml +++ b/flatland/package.xml @@ -1,7 +1,7 @@ flatland - 1.1.3 + 1.4.1 This is the metapackage for flatland. diff --git a/flatland_msgs/CHANGELOG.rst b/flatland_msgs/CHANGELOG.rst new file mode 100644 index 00000000..da94b17e --- /dev/null +++ b/flatland_msgs/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package flatland_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.1 (2023-11-24) +------------------ +* CMake version required bump to fix ros build farm warning + + +1.4.0 (2023-11-22) +------------------ +* Version Bump + +1.3.3 (2023-02-06) +------------------ +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne diff --git a/flatland_msgs/CMakeLists.txt b/flatland_msgs/CMakeLists.txt index b37cb262..b4b623df 100644 --- a/flatland_msgs/CMakeLists.txt +++ b/flatland_msgs/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(flatland_msgs) # Ensure we're using c++11 diff --git a/flatland_msgs/LICENSE b/flatland_msgs/LICENSE new file mode 100644 index 00000000..32802b81 --- /dev/null +++ b/flatland_msgs/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, Avidbots Corp. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/flatland_msgs/package.xml b/flatland_msgs/package.xml index 2acddfad..84fd128d 100644 --- a/flatland_msgs/package.xml +++ b/flatland_msgs/package.xml @@ -1,7 +1,7 @@ flatland_msgs - 1.1.3 + 1.4.1 The flatland_msgs package BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_plugins/CHANGELOG.rst b/flatland_plugins/CHANGELOG.rst new file mode 100644 index 00000000..8b1bc563 --- /dev/null +++ b/flatland_plugins/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package flatland_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.1 (2023-11-24) +------------------ +* CMake version required bump to fix ros build farm warning + +1.4.0 (2023-11-22) +------------------ +* Merge pull request `#103 `_ adding +* Contributors: Marc Gallant, Marc Bosch-Jorge + +1.3.3 (2023-02-06) +------------------ +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index edb26ac5..dcac337d 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -1,8 +1,8 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.11) project(flatland_plugins) -# Ensure we're using c++11 -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/flatland_plugins/LICENSE b/flatland_plugins/LICENSE new file mode 100644 index 00000000..32802b81 --- /dev/null +++ b/flatland_plugins/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, Avidbots Corp. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/flatland_plugins/flatland_plugins.xml b/flatland_plugins/flatland_plugins.xml index 7b9f985a..d3d26571 100644 --- a/flatland_plugins/flatland_plugins.xml +++ b/flatland_plugins/flatland_plugins.xml @@ -8,6 +8,9 @@ Flatland differential drive plugin + + Flatland imu plugin + Publish body transformations diff --git a/flatland_plugins/include/flatland_plugins/bool_sensor.h b/flatland_plugins/include/flatland_plugins/bool_sensor.h index a4d0d335..95c34872 100644 --- a/flatland_plugins/include/flatland_plugins/bool_sensor.h +++ b/flatland_plugins/include/flatland_plugins/bool_sensor.h @@ -1,100 +1,100 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name bool_sensor.h - * @brief Boolean Sensor plugin - publishes true if body is in collision - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_BOOL_SENSOR_H -#define FLATLAND_PLUGINS_BOOL_SENSOR_H - -using namespace flatland_server; - -namespace flatland_plugins { - -/** - * This class defines a bumper plugin that is used to publish the collisions - * states of bodies in the model - */ -class BoolSensor : public ModelPlugin { - public: - Body *body_; ///< The body to check collisions with - double update_rate_; ///< rate to publish message at - - UpdateTimer update_timer_; ///< for managing update rate - int collisions_ = 0; ///< Current number of collisions - bool hit_something_ = false; ///< "latch" var to ensure all hits published - - ros::Publisher publisher_; ///< For publishing the collisions - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; - - /** - * @brief Called when just after physics update to publish state - * @param[in] timekeeper Object managing the simulation time - */ - void AfterPhysicsStep(const Timekeeper &timekeeper) override; - - /** - * @brief A method that is called for all Box2D begin contacts - * @param[in] contact Box2D contact - */ - void BeginContact(b2Contact *contact) override; - - /** - * @brief A method that is called for all Box2D end contacts - * @param[in] contact Box2D contact - */ - void EndContact(b2Contact *contact) override; -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name bool_sensor.h + * @brief Boolean Sensor plugin - publishes true if body is in collision + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_BOOL_SENSOR_H +#define FLATLAND_PLUGINS_BOOL_SENSOR_H + +using namespace flatland_server; + +namespace flatland_plugins { + +/** + * This class defines a bumper plugin that is used to publish the collisions + * states of bodies in the model + */ +class BoolSensor : public ModelPlugin { + public: + Body *body_; ///< The body to check collisions with + double update_rate_; ///< rate to publish message at + + UpdateTimer update_timer_; ///< for managing update rate + int collisions_ = 0; ///< Current number of collisions + bool hit_something_ = false; ///< "latch" var to ensure all hits published + + ros::Publisher publisher_; ///< For publishing the collisions + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; + + /** + * @brief Called when just after physics update to publish state + * @param[in] timekeeper Object managing the simulation time + */ + void AfterPhysicsStep(const Timekeeper &timekeeper) override; + + /** + * @brief A method that is called for all Box2D begin contacts + * @param[in] contact Box2D contact + */ + void BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) override; + + /** + * @brief A method that is called for all Box2D end contacts + * @param[in] contact Box2D contact + */ + void EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) override; +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/bumper.h b/flatland_plugins/include/flatland_plugins/bumper.h index cedd55b1..14a3750a 100644 --- a/flatland_plugins/include/flatland_plugins/bumper.h +++ b/flatland_plugins/include/flatland_plugins/bumper.h @@ -1,132 +1,159 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name bumper.h - * @brief Bumper plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_BUMPER_H -#define FLATLAND_PLUGINS_BUMPER_H - -using namespace flatland_server; - -namespace flatland_plugins { - -/** - * This class defines a bumper plugin that is used to publish the collisions - * states of bodies in the model - */ -class Bumper : public ModelPlugin { - public: - struct ContactState { - int num_count; ///< stores number of times post solve is called - double sum_normal_impulses[2]; ///< sum of impulses for averaging later - double sum_tangential_impulses[2]; ///< sum of impulses for averaging later - b2Vec2 points[2]; ///< Box2D collision points, max of 2 from Box2D - b2Vec2 normal; ///< normal of collision points, all points have same normal - int normal_sign; ///< for flipping direction of normal when necessary - - Body *body_A; ///< the body of the model involved in the collision - Body *body_B; ///< the other body involved in the collision - Entity *entity_B; /// the entity the other body belongs to - - ContactState(); ///< initializes counters and sums - void Reset(); ///< Reset counter and sums - }; - - std::string topic_name_; - std::string world_frame_id_; ///< name of the world frame id - std::vector excluded_bodies_; ///< bodies to ignore - /// whether to publish all collisions, or strictly adhere to update rate - bool publish_all_collisions_; - double update_rate_; ///< rate to publish message at - - UpdateTimer update_timer_; ///< for managing update rate - - /// For keeping track of contacts - std::map contact_states_; - ros::Publisher collisions_publisher_; ///< For publishing the collisions - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; - - /** - * @brief Called when just before physics update - * @param[in] timekeeper Object managing the simulation time - */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; - - /** - * @brief Called when just after physics update - * @param[in] timekeeper Object managing the simulation time - */ - void AfterPhysicsStep(const Timekeeper &timekeeper) override; - - /** - * @brief A method that is called for all Box2D begin contacts - * @param[in] contact Box2D contact - */ - void BeginContact(b2Contact *contact) override; - - /** - * @brief A method that is called for all Box2D end contacts - * @param[in] contact Box2D contact - */ - void EndContact(b2Contact *contact) override; - - /* - * @brief A method that is called for Box2D presolve - * @param[in] contact Box2D contact - * @param[in] oldManifold Manifold from the previous iteration - */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) override; -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name bumper.h + * @brief Bumper plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_BUMPER_H +#define FLATLAND_PLUGINS_BUMPER_H + +using namespace flatland_server; + +namespace flatland_plugins { + +/** + * This class defines a bumper plugin that is used to publish the collisions + * states of bodies in the model + */ +class Bumper : public ModelPlugin { + public: + /// Opaque key uniquely identifying a contact between two shapes (order-independent) + struct ContactKey { + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + + static uint64_t Encode(b2ShapeId id) { + return (static_cast(id.index1) << 32) | + (static_cast(id.generation) << 16) | + static_cast(id.world0); + } + bool operator<(const ContactKey &o) const { + uint64_t a = Encode(shapeIdA), b = Encode(shapeIdB); + uint64_t oa = Encode(o.shapeIdA), ob = Encode(o.shapeIdB); + if (a > b) std::swap(a, b); + if (oa > ob) std::swap(oa, ob); + if (a != oa) return a < oa; + return b < ob; + } + }; + + struct ContactState { + int num_count; ///< number of hit events accumulated + double sum_speed; ///< sum of approach speeds + b2Vec2 point; ///< last contact point + b2Vec2 normal; ///< last contact normal + + Body *body_A; ///< the body of the model involved in the collision + Body *body_B; ///< the other body involved in the collision + Entity *entity_B; ///< the entity the other body belongs to + + ContactState(); ///< initializes counters + void Reset(); ///< Reset counter and sums + }; + + std::string topic_name_; + std::string world_frame_id_; ///< name of the world frame id + std::vector excluded_bodies_; ///< bodies to ignore + /// whether to publish all collisions, or strictly adhere to update rate + bool publish_all_collisions_; + double update_rate_; ///< rate to publish message at + + UpdateTimer update_timer_; ///< for managing update rate + + /// For keeping track of contacts + std::map contact_states_; + ros::Publisher collisions_publisher_; ///< For publishing the collisions + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; + + /** + * @brief Called when just before physics update + * @param[in] timekeeper Object managing the simulation time + */ + void BeforePhysicsStep(const Timekeeper &timekeeper) override; + + /** + * @brief Called when just after physics update + * @param[in] timekeeper Object managing the simulation time + */ + void AfterPhysicsStep(const Timekeeper &timekeeper) override; + + /** + * @brief A method that is called for all Box2D begin contacts + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + */ + void BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) override; + + /** + * @brief A method that is called for all Box2D end contacts + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + */ + void EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) override; + + /* + * @brief A method that is called for Box2D contact hit events + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + * @param[in] point World-space contact point + * @param[in] normal Contact normal + * @param[in] approachSpeed Approach speed + */ + void OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, + float approachSpeed) override; +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h index 45e07f65..f1c5fabb 100644 --- a/flatland_plugins/include/flatland_plugins/diff_drive.h +++ b/flatland_plugins/include/flatland_plugins/diff_drive.h @@ -1,107 +1,112 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name diff_drive.h - * @brief Diff drive plugin - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_DIFFDRIVE_H -#define FLATLAND_PLUGINS_DIFFDRIVE_H - -using namespace flatland_server; - -namespace flatland_plugins { - -class DiffDrive : public flatland_server::ModelPlugin { - public: - ros::Subscriber twist_sub_; - ros::Publisher odom_pub_; - ros::Publisher ground_truth_pub_; - ros::Publisher twist_pub_; - Body* body_; - geometry_msgs::Twist twist_msg_; - nav_msgs::Odometry odom_msg_; - nav_msgs::Odometry ground_truth_msg_; - UpdateTimer update_timer_; - tf::TransformBroadcaster tf_broadcaster; ///< For publish ROS TF - bool enable_odom_pub_; ///< YAML parameter to enable odom publishing - bool enable_twist_pub_; ///< YAML parameter to enable twist publishing - DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints - DynamicsLimits linear_dynamics_; ///< Linear dynamics constraints - double angular_velocity_ = 0.0; - double linear_velocity_ = 0.0; - - std::default_random_engine rng_; - std::array, 6> noise_gen_; - - /** - * @name OnInitialize - * @brief override the BeforePhysicsStep method - * @param[in] config The plugin YAML node - */ - void OnInitialize(const YAML::Node& config) override; - /** - * @name BeforePhysicsStep - * @brief override the BeforePhysicsStep method - * @param[in] config The plugin YAML node - */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; - /** - * @name TwistCallback - * @brief callback to apply twist (velocity and omega) - * @param[in] timestep how much the physics time will increment - */ - void TwistCallback(const geometry_msgs::Twist& msg); -}; -}; - -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name diff_drive.h + * @brief Diff drive plugin + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_DIFFDRIVE_H +#define FLATLAND_PLUGINS_DIFFDRIVE_H + +using namespace flatland_server; + +namespace flatland_plugins { + +class DiffDrive : public flatland_server::ModelPlugin { + public: + ros::Subscriber twist_sub_; + ros::Publisher odom_pub_; + ros::Publisher ground_truth_pub_; + ros::Publisher twist_pub_; + Body* body_; + geometry_msgs::Twist twist_msg_; + nav_msgs::Odometry odom_msg_; + nav_msgs::Odometry ground_truth_msg_; + UpdateTimer update_timer_; + tf::TransformBroadcaster tf_broadcaster; ///< For publish ROS TF + bool enable_odom_pub_; ///< YAML parameter to enable odom publishing + bool enable_odom_tf_pub_; ///< YAML parameter to enable odom tf publishing + bool enable_twist_pub_; ///< YAML parameter to enable twist publishing + bool enable_ground_truth_pub_; ///< YAML parameter to enable ground truth publishing + bool twist_in_local_frame_; ///< YAML parameter to publish velocity in local + /// frame. Original diff drive plugin publishes + /// local velocity wrt to odom frame + DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints + DynamicsLimits linear_dynamics_; ///< Linear dynamics constraints + double angular_velocity_ = 0.0; + double linear_velocity_ = 0.0; + + std::default_random_engine rng_; + std::array, 6> noise_gen_; + + /** + * @name OnInitialize + * @brief override the BeforePhysicsStep method + * @param[in] config The plugin YAML node + */ + void OnInitialize(const YAML::Node& config) override; + /** + * @name BeforePhysicsStep + * @brief override the BeforePhysicsStep method + * @param[in] config The plugin YAML node + */ + void BeforePhysicsStep(const Timekeeper& timekeeper) override; + /** + * @name TwistCallback + * @brief callback to apply twist (velocity and omega) + * @param[in] timestep how much the physics time will increment + */ + void TwistCallback(const geometry_msgs::Twist& msg); +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/dynamics_limits.h b/flatland_plugins/include/flatland_plugins/dynamics_limits.h index 2e859e25..01271b98 100644 --- a/flatland_plugins/include/flatland_plugins/dynamics_limits.h +++ b/flatland_plugins/include/flatland_plugins/dynamics_limits.h @@ -1,100 +1,100 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2021 Avidbots Corp. - * @name Dynamics_Limits.h - * @brief A generic acceleration, deceleration and velocity bounding utility class - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef FLATLAND_PLUGINS_DYNAMICS_LIMITS_H -#define FLATLAND_PLUGINS_DYNAMICS_LIMITS_H - -#include -#include - -namespace flatland_plugins { - -/** - * This class implements the model plugin class and provides laser data - * for the given configurations - */ -class DynamicsLimits { - public: - double acceleration_limit_ = 0.0; // Maximum rate of change in velocity in the direction away from zero. Zero value disables this limit. - double deceleration_limit_ = 0.0; // Maximum rate of change in velocity in the direction towards zero. Zero value disables this limit. - double velocity_limit_ = 0.0; // Maximum rate of change in position (in either direction). Zero value disables this limit. - - - /** - * @brief blank constructor, no-op class - */ - DynamicsLimits() {}; - - /** - * @name Load configuration from a yaml object - * @brief Constructor from yaml configuration file node - * @param[in] YAML::Node& configuration node - */ - void Configure(const YAML::Node &config); - - /** - * @name Saturate - * @brief Apply dynamics limits - * @param[in] config The plugin YAML node - */ - static double Saturate(double in, double lower, double upper); - - /** - * @name Apply - * @brief Apply dynamics limits based on class configured limits - * @param[in] velocity The current velocity (units/second) - * @param[in] target_velocity The target velocity requested (units/second) - * @param[in] timestep The timestep (seconds) (used to calculate acceleration from delta velocity) - * @return The new velocity result after target velocity has been subjected to limits - */ - double Limit(double velocity, double target_velocity, double timestep); - -}; - -}; - -#endif // FLATLAND_PLUGINS_DYNAMICS_LIMITS_H \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2021 Avidbots Corp. + * @name Dynamics_Limits.h + * @brief A generic acceleration, deceleration and velocity bounding utility class + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#ifndef FLATLAND_PLUGINS_DYNAMICS_LIMITS_H +#define FLATLAND_PLUGINS_DYNAMICS_LIMITS_H + +#include +#include + +namespace flatland_plugins { + +/** + * This class implements the model plugin class and provides laser data + * for the given configurations + */ +class DynamicsLimits { + public: + double acceleration_limit_ = 0.0; // Maximum rate of change in velocity in the direction away from zero. Zero value disables this limit. + double deceleration_limit_ = 0.0; // Maximum rate of change in velocity in the direction towards zero. Zero value disables this limit. + double velocity_limit_ = 0.0; // Maximum rate of change in position (in either direction). Zero value disables this limit. + + + /** + * @brief blank constructor, no-op class + */ + DynamicsLimits() {}; + + /** + * @name Load configuration from a yaml object + * @brief Constructor from yaml configuration file node + * @param[in] YAML::Node& configuration node + */ + void Configure(const YAML::Node &config); + + /** + * @name Saturate + * @brief Apply dynamics limits + * @param[in] config The plugin YAML node + */ + static double Saturate(double in, double lower, double upper); + + /** + * @name Apply + * @brief Apply dynamics limits based on class configured limits + * @param[in] velocity The current velocity (units/second) + * @param[in] target_velocity The target velocity requested (units/second) + * @param[in] timestep The timestep (seconds) (used to calculate acceleration from delta velocity) + * @return The new velocity result after target velocity has been subjected to limits + */ + double Limit(double velocity, double target_velocity, double timestep); + +}; + +}; + +#endif // FLATLAND_PLUGINS_DYNAMICS_LIMITS_H diff --git a/flatland_plugins/include/flatland_plugins/gps.h b/flatland_plugins/include/flatland_plugins/gps.h index 9b1c6cb7..da2f0eca 100644 --- a/flatland_plugins/include/flatland_plugins/gps.h +++ b/flatland_plugins/include/flatland_plugins/gps.h @@ -1,82 +1,82 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_GPS_H -#define FLATLAND_PLUGINS_GPS_H - -using namespace flatland_server; - -namespace flatland_plugins { - -/** - * This class simulates a GPS receiver in Flatland - */ -class Gps : public ModelPlugin { - public: - std::string topic_; ///< topic name to publish the GPS fix - std::string frame_id_; ///< GPS frame ID - Body *body_; ///< body the simulated GPS antenna attaches to - Pose origin_; ///< GPS sensor frame w.r.t the body - double ref_lat_rad_; ///< latitude in radians corresponding to (0, 0) in map - /// frame - double ref_lon_rad_; ///< longitude in radians corresponding to (0, 0) in map - /// frame - double ref_ecef_x_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double ref_ecef_y_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double ref_ecef_z_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double update_rate_; ///< GPS fix publish rate - bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body - - static double WGS84_A; ///< Earth's major axis length - static double WGS84_E2; ///< Square of Earth's first eccentricity - - ros::Publisher fix_publisher_; ///< GPS fix topic publisher - tf::TransformBroadcaster tf_broadcaster_; ///< broadcast GPS frame - geometry_msgs::TransformStamped gps_tf_; ///< tf from body to GPS frame - sensor_msgs::NavSatFix gps_fix_; ///< message for publishing output - UpdateTimer update_timer_; ///< for controlling update rate - - Eigen::Matrix3f m_body_to_gps_; ///< tf from body to GPS - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; - - /** - * @brief Called when just before physics update - * @param[in] timekeeper Object managing the simulation time - */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; - - /** - * @brief Helper function to extract the paramters from the YAML Node - * @param[in] config Plugin YAML Node - */ - void ParseParameters(const YAML::Node &config); - - /** - * @brief Method to compute ECEF coordinates of reference - * latitude and longitude, assuming zero altitude - */ - void ComputeReferenceEcef(); - - /** - * @brief Method that updates the current state of the GPS fix output - * for publishing - */ - void UpdateFix(); -}; -} - -#endif // FLATLAND_PLUGINS_GPS_H +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_GPS_H +#define FLATLAND_PLUGINS_GPS_H + +using namespace flatland_server; + +namespace flatland_plugins { + +/** + * This class simulates a GPS receiver in Flatland + */ +class Gps : public ModelPlugin { + public: + std::string topic_; ///< topic name to publish the GPS fix + std::string frame_id_; ///< GPS frame ID + Body *body_; ///< body the simulated GPS antenna attaches to + Pose origin_; ///< GPS sensor frame w.r.t the body + double ref_lat_rad_; ///< latitude in radians corresponding to (0, 0) in map + /// frame + double ref_lon_rad_; ///< longitude in radians corresponding to (0, 0) in map + /// frame + double ref_ecef_x_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double ref_ecef_y_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double ref_ecef_z_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double update_rate_; ///< GPS fix publish rate + bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body + + static double WGS84_A; ///< Earth's major axis length + static double WGS84_E2; ///< Square of Earth's first eccentricity + + ros::Publisher fix_publisher_; ///< GPS fix topic publisher + tf::TransformBroadcaster tf_broadcaster_; ///< broadcast GPS frame + geometry_msgs::TransformStamped gps_tf_; ///< tf from body to GPS frame + sensor_msgs::NavSatFix gps_fix_; ///< message for publishing output + UpdateTimer update_timer_; ///< for controlling update rate + + Eigen::Matrix3f m_body_to_gps_; ///< tf from body to GPS + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; + + /** + * @brief Called when just before physics update + * @param[in] timekeeper Object managing the simulation time + */ + void BeforePhysicsStep(const Timekeeper &timekeeper) override; + + /** + * @brief Helper function to extract the paramters from the YAML Node + * @param[in] config Plugin YAML Node + */ + void ParseParameters(const YAML::Node &config); + + /** + * @brief Method to compute ECEF coordinates of reference + * latitude and longitude, assuming zero altitude + */ + void ComputeReferenceEcef(); + + /** + * @brief Method that updates the current state of the GPS fix output + * for publishing + */ + void UpdateFix(); +}; +} + +#endif // FLATLAND_PLUGINS_GPS_H diff --git a/flatland_plugins/include/flatland_plugins/imu.h b/flatland_plugins/include/flatland_plugins/imu.h new file mode 100644 index 00000000..fa90143a --- /dev/null +++ b/flatland_plugins/include/flatland_plugins/imu.h @@ -0,0 +1,102 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name imu.h + * @brief IMU plugin + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_IMU_H +#define FLATLAND_PLUGINS_IMU_H + +using namespace flatland_server; + +namespace flatland_plugins { + +class Imu : public flatland_server::ModelPlugin { + public: + ros::Publisher imu_pub_; + ros::Publisher ground_truth_pub_; + Body* body_; + sensor_msgs::Imu imu_msg_; + sensor_msgs::Imu ground_truth_msg_; + UpdateTimer update_timer_; + bool enable_imu_pub_; ///< YAML parameter to enable odom publishing + + std::default_random_engine rng_; + std::array, 9> noise_gen_; + geometry_msgs::TransformStamped imu_tf_; ///< tf from body to IMU frame + tf::TransformBroadcaster tf_broadcaster_; ///< broadcast IMU frame + std::string imu_frame_id_; + bool broadcast_tf_; + b2Vec2 linear_vel_local_prev; + double pub_rate_; + /** + * @name OnInitialize + * @brief override the BeforePhysicsStep method + * @param[in] config The plugin YAML node + */ + void OnInitialize(const YAML::Node& config) override; + /** + * @name AfterPhysicsStep + * @brief override the AfterPhysicsStep method + * @param[in] timekeeper Tracks time in flatland + */ + void AfterPhysicsStep(const Timekeeper& timekeeper) override; + /** + * @name TwistCallback + * @brief callback to apply twist (velocity and omega) + * @param[in] timestep how much the physics time will increment + */ + void TwistCallback(const geometry_msgs::Twist& msg); +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 68d83452..bc07c14b 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -1,171 +1,148 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name laser.h - * @brief Laser plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_LASER_H -#define FLATLAND_PLUGINS_LASER_H - -using namespace flatland_server; - -namespace flatland_plugins { - -/** - * This class implements the model plugin class and provides laser data - * for the given configurations - */ -class Laser : public ModelPlugin { - public: - std::string topic_; ///< topic name to publish the laser scan - Body *body_; ///< body the laser frame attaches to - Pose origin_; ///< laser frame w.r.t the body - double range_; ///< laser max range - double noise_std_dev_; ///< noise std deviation - double max_angle_; /// < laser max angle - double min_angle_; ///< laser min angle - double increment_; ///< laser angle increment - double update_rate_; ///< the rate laser scan will be published - std::string frame_id_; ///< laser frame id name - bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body - uint16_t layers_bits_; ///< for setting the layers where laser will function - ThreadPool pool_; ///< ThreadPool for managing concurrent scan threads - - /* - * for setting reflectance layers. if the laser hits those layers, - * intensity will be high (255) - */ - uint16_t reflectance_layers_bits_; - - std::default_random_engine rng_; ///< random generator - std::normal_distribution noise_gen_; ///< gaussian noise generator - - Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser - Eigen::Matrix3f m_world_to_body_; ///< tf from world to body - Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser - Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame - Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame - Eigen::Vector3f v_zero_point_; ///< point representing (0,0) - Eigen::Vector3f v_world_laser_origin_; ///< (0,0) in the laser frame - sensor_msgs::LaserScan laser_scan_; ///< for publishing laser scan - - ros::Publisher scan_publisher_; ///< ros laser topic publisher - tf::TransformBroadcaster tf_broadcaster_; ///< broadcast laser frame - geometry_msgs::TransformStamped laser_tf_; ///< tf from body to laser frame - UpdateTimer update_timer_; ///< for controlling update rate - - /** - * @brief Constructor to start the threadpool with N+1 threads - */ - Laser() : pool_(std::thread::hardware_concurrency() + 1) { - ROS_INFO_STREAM("Laser plugin loaded with " - << (std::thread::hardware_concurrency() + 1) << " threads"); - }; - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; - - /** - * @brief Called when just before physics update - * @param[in] timekeeper Object managing the simulation time - */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; - - /** - * @brief Method that contains all of the laser range calculations - */ - void ComputeLaserRanges(); - - /** - * @brief helper function to extract the paramters from the YAML Node - * @param[in] config Plugin YAML Node - */ - void ParseParameters(const YAML::Node &config); -}; - -/** - * This class handles the b2RayCastCallback ReportFixture method - * allowing each thread to access its own callback object - */ -class LaserCallback : public b2RayCastCallback { - public: - bool did_hit_ = false; ///< Box2D ray trace checking if ray hits anything - float fraction_ = 0; ///< Box2D ray trace fraction - float intensity_ = 0; ///< Intensity of raytrace collision - Laser *parent_; ///< The parent Laser plugin - - /** - * Default constructor to assign parent object - */ - LaserCallback(Laser *parent) : parent_(parent){}; - - /** - * @brief Box2D raytrace call back method required for implementing the - * b2RayCastCallback abstract class - * @param[in] fixture Fixture the ray hits - * @param[in] point Point the ray hits the fixture - * @param[in] normal Vector indicating the normal at the point hit - * @param[in] fraction Fraction of ray length at hit point - */ - float ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) override; -}; -}; - -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name laser.h + * @brief Laser plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_LASER_H +#define FLATLAND_PLUGINS_LASER_H + +using namespace flatland_server; + +namespace flatland_plugins { + +/** + * This class implements the model plugin class and provides laser data + * for the given configurations + */ +class Laser : public ModelPlugin { + public: + std::string topic_; ///< topic name to publish the laser scan + Body *body_; ///< body the laser frame attaches to + Pose origin_; ///< laser frame w.r.t the body + double range_; ///< laser max range + double noise_std_dev_; ///< noise std deviation + double max_angle_; /// < laser max angle + double min_angle_; ///< laser min angle + double increment_; ///< laser angle increment + double update_rate_; ///< the rate laser scan will be published + std::string frame_id_; ///< laser frame id name + bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body + uint16_t layers_bits_; ///< for setting the layers where laser will function + + /* + * for setting reflectance layers. if the laser hits those layers, + * intensity will be high (255) + */ + uint16_t reflectance_layers_bits_; + + std::default_random_engine rng_; ///< random generator + std::normal_distribution noise_gen_; ///< gaussian noise generator + + Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser + Eigen::Matrix3f m_world_to_body_; ///< tf from world to body + Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser + Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame + Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame + Eigen::Vector3f v_zero_point_; ///< point representing (0,0) + Eigen::Vector3f v_world_laser_origin_; ///< (0,0) in the laser frame + sensor_msgs::LaserScan laser_scan_; ///< for publishing laser scan + + ros::Publisher scan_publisher_; ///< ros laser topic publisher + tf::TransformBroadcaster tf_broadcaster_; ///< broadcast laser frame + geometry_msgs::TransformStamped laser_tf_; ///< tf from body to laser frame + UpdateTimer update_timer_; ///< for controlling update rate + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; + + /** + * @brief Called when just before physics update + * @param[in] timekeeper Object managing the simulation time + */ + void BeforePhysicsStep(const Timekeeper &timekeeper) override; + + /** + * @brief Method that contains all of the laser range calculations + */ + void ComputeLaserRanges(); + + /** + * @brief helper function to extract the paramters from the YAML Node + * @param[in] config Plugin YAML Node + */ + void ParseParameters(const YAML::Node &config); +}; + +/** + * Context passed to the Box2D v3 ray cast free function + */ +struct LaserRayContext { + bool did_hit = false; + float fraction = 0.0f; + float intensity = 0.0f; + uint16_t layers_bits; + uint16_t reflectance_layers_bits; +}; + +/// Box2D v3 ray cast callback (free function) +float LaserRayCastFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, + float fraction, void *context); + +}; // namespace flatland_plugins + +#endif diff --git a/flatland_plugins/include/flatland_plugins/model_tf_publisher.h b/flatland_plugins/include/flatland_plugins/model_tf_publisher.h index fd18e679..3599def3 100644 --- a/flatland_plugins/include/flatland_plugins/model_tf_publisher.h +++ b/flatland_plugins/include/flatland_plugins/model_tf_publisher.h @@ -1,89 +1,89 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_tf_publisher.h - * @brief Publish tf in robots - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H -#define FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H - -using namespace flatland_server; - -namespace flatland_plugins { - -/** - * This class implements the model plugin and provides the functionality of - * publishing ROS TF transformations for the bodies inside a model - */ -class ModelTfPublisher : public ModelPlugin { - public: - std::string world_frame_id_; ///< name of the world frame id - bool publish_tf_world_; ///< if to publish the world position of the bodies - std::vector excluded_bodies_; ///< list of bodies to ignore - Body *reference_body_; ///< body used as a reference to other bodies - double update_rate_; ///< publish rate - - tf::TransformBroadcaster tf_broadcaster; ///< For publish ROS TF - UpdateTimer update_timer_; ///< for managing update rate - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; - - /** - * @brief Called when just before physics update - * @param[in] timekeeper Object managing the simulation time - */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_tf_publisher.h + * @brief Publish tf in robots + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H +#define FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H + +using namespace flatland_server; + +namespace flatland_plugins { + +/** + * This class implements the model plugin and provides the functionality of + * publishing ROS TF transformations for the bodies inside a model + */ +class ModelTfPublisher : public ModelPlugin { + public: + std::string world_frame_id_; ///< name of the world frame id + bool publish_tf_world_; ///< if to publish the world position of the bodies + std::vector excluded_bodies_; ///< list of bodies to ignore + Body *reference_body_; ///< body used as a reference to other bodies + double update_rate_; ///< publish rate + + tf::TransformBroadcaster tf_broadcaster; ///< For publish ROS TF + UpdateTimer update_timer_; ///< for managing update rate + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; + + /** + * @brief Called when just before physics update + * @param[in] timekeeper Object managing the simulation time + */ + void BeforePhysicsStep(const Timekeeper &timekeeper) override; +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/tricycle_drive.h b/flatland_plugins/include/flatland_plugins/tricycle_drive.h index cacb6aff..a2db3a31 100644 --- a/flatland_plugins/include/flatland_plugins/tricycle_drive.h +++ b/flatland_plugins/include/flatland_plugins/tricycle_drive.h @@ -1,146 +1,146 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name tricycle.h - * @brief Tricycle plugin - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_TRICYCLE_DRIVE_H -#define FLATLAND_PLUGINS_TRICYCLE_DRIVE_H - -using namespace flatland_server; -using namespace std; - -namespace flatland_plugins { - -class TricycleDrive : public flatland_server::ModelPlugin { - public: - Body* body_; - Joint* front_wj_; ///< front wheel joint - Joint* rear_left_wj_; ///< rear left wheel joint - Joint* rear_right_wj_; ///< rear right wheel joint - double axel_track_; ///< normal distrance between the rear two wheels - double wheelbase_; ///< distance between the front and rear wheel - b2Vec2 rear_center_; ///< middle point between the two rear wheels - bool invert_steering_angle_; ///< whether to invert steering angle - double max_steer_angle_; ///< max abs. steering allowed [rad] - double max_steer_velocity_; ///< max abs. steering velocity [rad/s] - double max_steer_acceleration_; ///< max abs. steering acceleration [rad/s^2] - DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints - DynamicsLimits linear_dynamics_; ///< Linear dynamics constraints - double delta_command_; ///< The current target (commanded) wheel angle - double theta_f_; ///< The current angular offset of the front wheel - double d_delta_; ///< The current angular speed of the front wheel - double v_f_ = 0.0; ///< The current velocity at the front wheel - - geometry_msgs::Twist twist_msg_; - nav_msgs::Odometry odom_msg_; - nav_msgs::Odometry ground_truth_msg_; - ros::Subscriber twist_sub_; - ros::Publisher odom_pub_; - ros::Publisher ground_truth_pub_; - - UpdateTimer update_timer_; - - default_random_engine rng_; - array, 6> noise_gen_; - - /** - * @name OnInitialize - * @brief initialize the bicycle plugin - * @param world_file The path to the world.yaml file - */ - void OnInitialize(const YAML::Node& config) override; - - /** - * @brief This is a helper function that is used to valid and extract - * parameters from joints - */ - void ComputeJoints(); - - /** - * @brief Updates the vehicle state given the twist command; - * overrides the BeforePhysicsStep method - * @details Uses a 2nd-order approximation of the steering & drive systems. - * Does not account for dynamics such as: - * - Motor winding current/rpm/torque behaviour - * - Other motor PID controllers in the loop - * - Communication delays - * - Any mechanical lag in the drive mechanism (chains, inertia) - * - Measurement dynamics other than Gaussian noise - * A separate plugin which subscribes to different messages - * could be used for more accurate modelling. - * - * References: - * Some notation and ideas borrowed from - * http://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf - */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; - - /** - * @name TwistCallback - * @brief callback to apply twist (velocity and omega) - * @param[in] timestep how much the physics time will increment - */ - void TwistCallback(const geometry_msgs::Twist& msg); - - /** - * @brief Saturates the input between the lower and upper limits - * @param[in] in: value to saturate - * @param[in] lower: lower limit of saturation bound - * @param[in] upper: upper limit of saturation bound - * @return input value capped between lower and upper - */ - double Saturate(double in, double lower, double upper); -}; -} - -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name tricycle.h + * @brief Tricycle plugin + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_TRICYCLE_DRIVE_H +#define FLATLAND_PLUGINS_TRICYCLE_DRIVE_H + +using namespace flatland_server; +using namespace std; + +namespace flatland_plugins { + +class TricycleDrive : public flatland_server::ModelPlugin { + public: + Body* body_; + Joint* front_wj_; ///< front wheel joint + Joint* rear_left_wj_; ///< rear left wheel joint + Joint* rear_right_wj_; ///< rear right wheel joint + double axel_track_; ///< normal distrance between the rear two wheels + double wheelbase_; ///< distance between the front and rear wheel + b2Vec2 rear_center_; ///< middle point between the two rear wheels + bool invert_steering_angle_; ///< whether to invert steering angle + double max_steer_angle_; ///< max abs. steering allowed [rad] + double max_steer_velocity_; ///< max abs. steering velocity [rad/s] + double max_steer_acceleration_; ///< max abs. steering acceleration [rad/s^2] + DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints + DynamicsLimits linear_dynamics_; ///< Linear dynamics constraints + double delta_command_; ///< The current target (commanded) wheel angle + double theta_f_; ///< The current angular offset of the front wheel + double d_delta_; ///< The current angular speed of the front wheel + double v_f_ = 0.0; ///< The current velocity at the front wheel + + geometry_msgs::Twist twist_msg_; + nav_msgs::Odometry odom_msg_; + nav_msgs::Odometry ground_truth_msg_; + ros::Subscriber twist_sub_; + ros::Publisher odom_pub_; + ros::Publisher ground_truth_pub_; + + UpdateTimer update_timer_; + + default_random_engine rng_; + array, 6> noise_gen_; + + /** + * @name OnInitialize + * @brief initialize the bicycle plugin + * @param world_file The path to the world.yaml file + */ + void OnInitialize(const YAML::Node& config) override; + + /** + * @brief This is a helper function that is used to valid and extract + * parameters from joints + */ + void ComputeJoints(); + + /** + * @brief Updates the vehicle state given the twist command; + * overrides the BeforePhysicsStep method + * @details Uses a 2nd-order approximation of the steering & drive systems. + * Does not account for dynamics such as: + * - Motor winding current/rpm/torque behaviour + * - Other motor PID controllers in the loop + * - Communication delays + * - Any mechanical lag in the drive mechanism (chains, inertia) + * - Measurement dynamics other than Gaussian noise + * A separate plugin which subscribes to different messages + * could be used for more accurate modelling. + * + * References: + * Some notation and ideas borrowed from + * http://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf + */ + void BeforePhysicsStep(const Timekeeper& timekeeper) override; + + /** + * @name TwistCallback + * @brief callback to apply twist (velocity and omega) + * @param[in] timestep how much the physics time will increment + */ + void TwistCallback(const geometry_msgs::Twist& msg); + + /** + * @brief Saturates the input between the lower and upper limits + * @param[in] in: value to saturate + * @param[in] lower: lower limit of saturation bound + * @param[in] upper: upper limit of saturation bound + * @return input value capped between lower and upper + */ + double Saturate(double in, double lower, double upper); +}; +} + +#endif diff --git a/flatland_plugins/include/flatland_plugins/tween.h b/flatland_plugins/include/flatland_plugins/tween.h index 84647bca..a40d15de 100644 --- a/flatland_plugins/include/flatland_plugins/tween.h +++ b/flatland_plugins/include/flatland_plugins/tween.h @@ -1,141 +1,141 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name tween.h - * @brief Tween plugin - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "tweeny.h" - -#ifndef FLATLAND_PLUGINS_TWEEN_H -#define FLATLAND_PLUGINS_TWEEN_H - -using namespace flatland_server; - -namespace flatland_plugins { - -class Tween : public flatland_server::ModelPlugin { - public: - Body* body_; // The body this plugin is attached to - Pose start_; // The start pose of the model - Pose delta_; // The maximum change - float duration_; // Seconds to enact change over - - ros::Subscriber trigger_sub_; // Handle forward/reverse trigger - bool triggered_ = false; // If true,animate forwards, otherwise backwards - - tweeny::tween tween_; // The tween object (x,y,theta) - - // The three different operating modes - enum class ModeType_ { - YOYO, // tween up to delta_, then down again, and repeat - LOOP, // tween up to delta_, then teleport back to start_ - ONCE, // tween up to delta_ then stay there - TRIGGER // tween forwards on "true", backward on "false" - }; - ModeType_ mode_; - static std::map mode_strings_; - - enum class EasingType_ { - linear, - quadraticIn, - quadraticOut, - quadraticInOut, - cubicIn, - cubicOut, - cubicInOut, - quarticIn, - quarticOut, - quarticInOut, - quinticIn, - quinticOut, - quinticInOut, - // sinuisodal, - exponentialIn, - exponentialOut, - exponentialInOut, - circularIn, - circularOut, - circularInOut, - backIn, - backOut, - backInOut, - elasticIn, - elasticOut, - elasticInOut, - bounceIn, - bounceOut, - bounceInOut - }; - static std::map easing_strings_; - - /** - * @name OnInitialize - * @brief override the BeforePhysicsStep method - * @param[in] config The plugin YAML node - */ - void OnInitialize(const YAML::Node& config) override; - /** - * @name BeforePhysicsStep - * @brief override the BeforePhysicsStep method - * @param[in] config The plugin YAML node - */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; - - /** - * @name TriggerCallback - * @brief Handles external tween triggers for mode "trigger" - * @param[in] The boolean message - */ - void TriggerCallback(const std_msgs::Bool& msg); -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name tween.h + * @brief Tween plugin + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "tweeny.h" + +#ifndef FLATLAND_PLUGINS_TWEEN_H +#define FLATLAND_PLUGINS_TWEEN_H + +using namespace flatland_server; + +namespace flatland_plugins { + +class Tween : public flatland_server::ModelPlugin { + public: + Body* body_; // The body this plugin is attached to + Pose start_; // The start pose of the model + Pose delta_; // The maximum change + float duration_; // Seconds to enact change over + + ros::Subscriber trigger_sub_; // Handle forward/reverse trigger + bool triggered_ = false; // If true,animate forwards, otherwise backwards + + tweeny::tween tween_; // The tween object (x,y,theta) + + // The three different operating modes + enum class ModeType_ { + YOYO, // tween up to delta_, then down again, and repeat + LOOP, // tween up to delta_, then teleport back to start_ + ONCE, // tween up to delta_ then stay there + TRIGGER // tween forwards on "true", backward on "false" + }; + ModeType_ mode_; + static std::map mode_strings_; + + enum class EasingType_ { + linear, + quadraticIn, + quadraticOut, + quadraticInOut, + cubicIn, + cubicOut, + cubicInOut, + quarticIn, + quarticOut, + quarticInOut, + quinticIn, + quinticOut, + quinticInOut, + // sinuisodal, + exponentialIn, + exponentialOut, + exponentialInOut, + circularIn, + circularOut, + circularInOut, + backIn, + backOut, + backInOut, + elasticIn, + elasticOut, + elasticInOut, + bounceIn, + bounceOut, + bounceInOut + }; + static std::map easing_strings_; + + /** + * @name OnInitialize + * @brief override the BeforePhysicsStep method + * @param[in] config The plugin YAML node + */ + void OnInitialize(const YAML::Node& config) override; + /** + * @name BeforePhysicsStep + * @brief override the BeforePhysicsStep method + * @param[in] config The plugin YAML node + */ + void BeforePhysicsStep(const Timekeeper& timekeeper) override; + + /** + * @name TriggerCallback + * @brief Handles external tween triggers for mode "trigger" + * @param[in] The boolean message + */ + void TriggerCallback(const std_msgs::Bool& msg); +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/update_timer.h b/flatland_plugins/include/flatland_plugins/update_timer.h index 3d2decc1..665dc2d7 100644 --- a/flatland_plugins/include/flatland_plugins/update_timer.h +++ b/flatland_plugins/include/flatland_plugins/update_timer.h @@ -1,81 +1,81 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name update_timer.h - * @brief For managing plugin update rates - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_PLUGINS_UPDATE_TIMER_H -#define FLATLAND_PLUGINS_UPDATE_TIMER_H - -#include -#include - -namespace flatland_plugins { - -class UpdateTimer { - public: - ros::Duration period_; ///< period of update - ros::Time last_update_time_; ///< last time the update occured - - /** - * @brief Update timer constructor - */ - UpdateTimer(); - - /** - * @brief Set the update rate - * @param[in] rate Rate in Hz - */ - void SetRate(double rate); - - /** - * Call this method to check if an update is required to keep with the - * set update rate - * @param[in] timekeeper The object that manages time for the simulation, - * update timers get the simulation as well as step size for calculation - */ - bool CheckUpdate(const flatland_server::Timekeeper &timekeeper); -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name update_timer.h + * @brief For managing plugin update rates + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_PLUGINS_UPDATE_TIMER_H +#define FLATLAND_PLUGINS_UPDATE_TIMER_H + +#include +#include + +namespace flatland_plugins { + +class UpdateTimer { + public: + ros::Duration period_; ///< period of update + ros::Time last_update_time_; ///< last time the update occured + + /** + * @brief Update timer constructor + */ + UpdateTimer(); + + /** + * @brief Set the update rate + * @param[in] rate Rate in Hz + */ + void SetRate(double rate); + + /** + * Call this method to check if an update is required to keep with the + * set update rate + * @param[in] timekeeper The object that manages time for the simulation, + * update timers get the simulation as well as step size for calculation + */ + bool CheckUpdate(const flatland_server::Timekeeper &timekeeper); +}; +}; + +#endif diff --git a/flatland_plugins/include/flatland_plugins/world_modifier.h b/flatland_plugins/include/flatland_plugins/world_modifier.h index f2f62d0d..45557f54 100644 --- a/flatland_plugins/include/flatland_plugins/world_modifier.h +++ b/flatland_plugins/include/flatland_plugins/world_modifier.h @@ -1,144 +1,146 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_modifier.h - * @brief Provide general functions to modify the world - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef WORLD_MODIFIER_H -#define WORLD_MODIFIER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -using namespace flatland_server; - -namespace flatland_plugins { - -// sub class for loading laser Param -struct LaserRead { - std::string name_; // name of the laser - b2Vec2 location_; // location of the laser in local frame - double range_; // range of the laser - double min_angle_; - double max_angle_; - double increment_; - LaserRead(std::string name, b2Vec2 location, double range, double min_angle, - double max_angle, double increment) - : name_(name), - location_(location), - range_(range), - min_angle_(min_angle), - max_angle_(max_angle), - increment_(increment) {} -}; - -struct RayTrace : public b2RayCastCallback { - bool is_hit_; - float fraction_; - uint16_t category_bits_; - RayTrace(uint16_t category_bits) - : is_hit_(false), category_bits_(category_bits) {} - float ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) override; -}; - -struct WorldModifier { - // private members - World *world_; // the world we are modifying - std::string layer_name_; - double wall_wall_dist_; - bool double_wall_; - Pose robot_ini_pose_; - - /* - * @brief based on the info regard old wall and d, calculate new obstacle's - * vertices - * @param[in] double d, the sign of this value determine which side of the wall - * will the new obstacle be added - * @param[in] b2Vec2 vertex1, old wall's vertex1 - * @param[in] b2Vec2 vertex2, old wall's vertex2 - * @param[out] b2EdgeShape new_wall, reference passed in to set the vertices - */ - void CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, - b2EdgeShape &new_wall); - - /* - * @brief add the new wall into the world - * @param[in] new_wall, the wall that's going to be added - */ - void AddWall(b2EdgeShape &new_wall); - - /* - * @brief add two side walls to make it a full obstacle - * @param[in] old_wall, the old wall where new wall is added on top to - * @param[in] new_wall, the new wall got added - */ - void AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall); - - /* - * @brief constructor for WorldModifier - * @param[in] world, the world that we are adding walls to - * @param[in] layer_name, which layer is the obstacle going to be added - * @param[in] wall_wall_dist, how thick is the obstacle - * @param[in] double_wall, whether add obstacle on both side or not - * @param[in] robot_ini_pose, the initial pose of the robot - */ - WorldModifier(flatland_server::World *world, std::string layer_name, - double wall_wall_dist, bool double_wall, Pose robot_ini_pose); - - /* - * @brief make a new wall in front of the old wall, also add two side walls to - * make a full object - * @param[in] b2EdgeShape *wall, old wall where new wall will be added on top - * to - */ - void AddFullWall(b2EdgeShape *wall); - -}; // class WorldModifier -}; // namespace flatland_server -#endif // WORLD_MODIFIER_H \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_modifier.h + * @brief Provide general functions to modify the world + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef WORLD_MODIFIER_H +#define WORLD_MODIFIER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +using namespace flatland_server; + +namespace flatland_plugins { + +// sub class for loading laser Param +struct LaserRead { + std::string name_; // name of the laser + b2Vec2 location_; // location of the laser in local frame + double range_; // range of the laser + double min_angle_; + double max_angle_; + double increment_; + LaserRead(std::string name, b2Vec2 location, double range, double min_angle, + double max_angle, double increment) + : name_(name), + location_(location), + range_(range), + min_angle_(min_angle), + max_angle_(max_angle), + increment_(increment) {} +}; + +struct RayTrace { + bool is_hit_; + float fraction_; + uint16_t category_bits_; + RayTrace(uint16_t category_bits) + : is_hit_(false), fraction_(0.0f), category_bits_(category_bits) {} +}; + +/// Box2D v3 ray cast free function for RayTrace +float RayTraceRayCastFcn(b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, + float fraction, void *context); + +struct WorldModifier { + // private members + World *world_; // the world we are modifying + std::string layer_name_; + double wall_wall_dist_; + bool double_wall_; + Pose robot_ini_pose_; + + /* + * @brief based on the info regard old wall and d, calculate new obstacle's + * vertices + * @param[in] double d, the sign of this value determine which side of the wall + * will the new obstacle be added + * @param[in] b2Vec2 vertex1, old wall's vertex1 + * @param[in] b2Vec2 vertex2, old wall's vertex2 + * @param[out] b2EdgeShape new_wall, reference passed in to set the vertices + */ + void CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, + b2Segment &new_wall); + + /* + * @brief add the new wall into the world + * @param[in] new_wall, the wall that's going to be added + */ + void AddWall(b2Segment &new_wall); + + /* + * @brief add two side walls to make it a full obstacle + * @param[in] old_wall, the old wall where new wall is added on top to + * @param[in] new_wall, the new wall got added + */ + void AddSideWall(b2Segment &old_wall, b2Segment &new_wall); + + /* + * @brief constructor for WorldModifier + * @param[in] world, the world that we are adding walls to + * @param[in] layer_name, which layer is the obstacle going to be added + * @param[in] wall_wall_dist, how thick is the obstacle + * @param[in] double_wall, whether add obstacle on both side or not + * @param[in] robot_ini_pose, the initial pose of the robot + */ + WorldModifier(flatland_server::World *world, std::string layer_name, + double wall_wall_dist, bool double_wall, Pose robot_ini_pose); + + /* + * @brief make a new wall in front of the old wall, also add two side walls to + * make a full object + * @param[in] b2EdgeShape *wall, old wall where new wall will be added on top + * to + */ + void AddFullWall(b2Segment *wall); + +}; // class WorldModifier +}; // namespace flatland_server +#endif // WORLD_MODIFIER_H diff --git a/flatland_plugins/include/flatland_plugins/world_random_wall.h b/flatland_plugins/include/flatland_plugins/world_random_wall.h index 3a0b35c8..e23f67a2 100644 --- a/flatland_plugins/include/flatland_plugins/world_random_wall.h +++ b/flatland_plugins/include/flatland_plugins/world_random_wall.h @@ -1,64 +1,64 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_random_wall.h - * @brief a simple plugin that add random walls on the field - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H -#define FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H - -using namespace flatland_server; - -namespace flatland_plugins { -class RandomWall : public WorldPlugin { - void OnInitialize(const YAML::Node &config) override; -}; -}; - -#endif // FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_random_wall.h + * @brief a simple plugin that add random walls on the field + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H +#define FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H + +using namespace flatland_server; + +namespace flatland_plugins { +class RandomWall : public WorldPlugin { + void OnInitialize(const YAML::Node &config) override; +}; +}; + +#endif // FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H diff --git a/flatland_plugins/package.xml b/flatland_plugins/package.xml index 772688e9..56431f22 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -1,7 +1,7 @@ flatland_plugins - 1.1.3 + 1.4.1 Default plugins for flatland BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_plugins/src/bool_sensor.cpp b/flatland_plugins/src/bool_sensor.cpp index 43820e68..e09aa753 100644 --- a/flatland_plugins/src/bool_sensor.cpp +++ b/flatland_plugins/src/bool_sensor.cpp @@ -1,140 +1,140 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name bool_sensor.h - * @brief BoolSensor plugin - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -void BoolSensor::OnInitialize(const YAML::Node &config) { - YamlReader reader(config); - - // defaults - std::string topic_name = reader.Get("topic", "bool_sensor"); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); - - // sensor defaults to the first model in the list - if (GetModel()->bodies_.size() == 0) { - throw YAMLException("You didn't provide any bodies for model" + - GetModel()->name_); - } - std::string body_name = - reader.Get("body", GetModel()->bodies_[0]->name_); - - reader.EnsureAccessedAllKeys(); - - // Load the body pointer - body_ = GetModel()->GetBody(body_name); - if (body_ == nullptr) { - throw YAMLException("Body with name \"" + body_name + "\" does not exist"); - } - - // Set the update timer - update_timer_.SetRate(update_rate_); - - // Init publisher - publisher_ = - nh_.advertise(GetModel()->NameSpaceTopic(topic_name), 1); - - ROS_DEBUG_NAMED("BoolSensor", - "Initialized with params: topic(%s) body(%s) " - "update_rate(%f)", - topic_name.c_str(), body_name.c_str(), update_rate_); -} - -void BoolSensor::AfterPhysicsStep(const Timekeeper &timekeeper) { - // Publish the boolean timer at the desired update rate - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - - // This logic allows collisions that occur and resolve faster than the update - // rate to result in at least one "true" publish - std_msgs::Bool msg; - if (hit_something_) { // We hit something since the last publish - msg.data = true; - hit_something_ = false; // Reset for next time - } else { // Publish current state as usual - if (collisions_ > 0) { - msg.data = true; - } else { - msg.data = false; - } - } - publisher_.publish(msg); -} - -void BoolSensor::BeginContact(b2Contact *contact) { - if (!FilterContact(contact)) return; - - // Skip collisions with other fixtures on this body - if (contact->GetFixtureA()->GetBody() == contact->GetFixtureB()->GetBody()) { - return; - } - - collisions_++; - hit_something_ = true; -} - -void BoolSensor::EndContact(b2Contact *contact) { - if (!FilterContact(contact)) return; - - // Skip collisions with other fixtures on this body - if (contact->GetFixtureA()->GetBody() == contact->GetFixtureB()->GetBody()) { - return; - } - - collisions_--; -} -} // End flatland_plugins namespace - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::BoolSensor, - flatland_server::ModelPlugin) +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name bool_sensor.h + * @brief BoolSensor plugin + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +void BoolSensor::OnInitialize(const YAML::Node &config) { + YamlReader reader(config); + + // defaults + std::string topic_name = reader.Get("topic", "bool_sensor"); + update_rate_ = reader.Get("update_rate", + std::numeric_limits::infinity()); + + // sensor defaults to the first model in the list + if (GetModel()->bodies_.size() == 0) { + throw YAMLException("You didn't provide any bodies for model" + + GetModel()->name_); + } + std::string body_name = + reader.Get("body", GetModel()->bodies_[0]->name_); + + reader.EnsureAccessedAllKeys(); + + // Load the body pointer + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name \"" + body_name + "\" does not exist"); + } + + // Set the update timer + update_timer_.SetRate(update_rate_); + + // Init publisher + publisher_ = + nh_.advertise(GetModel()->NameSpaceTopic(topic_name), 1); + + ROS_DEBUG_NAMED("BoolSensor", + "Initialized with params: topic(%s) body(%s) " + "update_rate(%f)", + topic_name.c_str(), body_name.c_str(), update_rate_); +} + +void BoolSensor::AfterPhysicsStep(const Timekeeper &timekeeper) { + // Publish the boolean timer at the desired update rate + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + + // This logic allows collisions that occur and resolve faster than the update + // rate to result in at least one "true" publish + std_msgs::Bool msg; + if (hit_something_) { // We hit something since the last publish + msg.data = true; + hit_something_ = false; // Reset for next time + } else { // Publish current state as usual + if (collisions_ > 0) { + msg.data = true; + } else { + msg.data = false; + } + } + publisher_.publish(msg); +} + +void BoolSensor::BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + if (!FilterContact(shapeIdA, shapeIdB)) return; + + // Skip collisions with other shapes on the same body + if (B2_ID_EQUALS(b2Shape_GetBody(shapeIdA), b2Shape_GetBody(shapeIdB))) { + return; + } + + collisions_++; + hit_something_ = true; +} + +void BoolSensor::EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + if (!FilterContact(shapeIdA, shapeIdB)) return; + + // Skip collisions with other shapes on the same body + if (B2_ID_EQUALS(b2Shape_GetBody(shapeIdA), b2Shape_GetBody(shapeIdB))) { + return; + } + + collisions_--; +} +} // End flatland_plugins namespace + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::BoolSensor, + flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/bumper.cpp b/flatland_plugins/src/bumper.cpp index 58c6d283..ada8adf4 100644 --- a/flatland_plugins/src/bumper.cpp +++ b/flatland_plugins/src/bumper.cpp @@ -1,284 +1,219 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name bumper.h - * @brief Bumper plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -Bumper::ContactState::ContactState() { Reset(); } - -void Bumper::ContactState::Reset() { - num_count = 0; - sum_normal_impulses[0] = 0; - sum_normal_impulses[1] = 0; - sum_tangential_impulses[0] = 0; - sum_tangential_impulses[1] = 0; -} - -void Bumper::OnInitialize(const YAML::Node &config) { - YamlReader reader(config); - - // defaults - world_frame_id_ = reader.Get("world_frame_id", "map"); - topic_name_ = reader.Get("topic", "collisions"); - publish_all_collisions_ = reader.Get("publish_all_collisions", true); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); - - std::vector excluded_body_names = - reader.GetList("exclude", {}, -1, -1); - - reader.EnsureAccessedAllKeys(); - - for (unsigned int i = 0; i < excluded_body_names.size(); i++) { - Body *body = GetModel()->GetBody(excluded_body_names[i]); - - if (body == nullptr) { - throw YAMLException("Body with name \"" + excluded_body_names[i] + - "\" does not exist"); - } else { - excluded_bodies_.push_back(body); - } - } - - update_timer_.SetRate(update_rate_); - collisions_publisher_ = - nh_.advertise(topic_name_, 1); - - ROS_DEBUG_NAMED("Bumper", - "Initialized with params: topic(%s) world_frame_id(%s) " - "publish_all_collisions(%d) update_rate(%f) exclude({%s})", - topic_name_.c_str(), world_frame_id_.c_str(), - publish_all_collisions_, update_rate_, - boost::algorithm::join(excluded_body_names, ",").c_str()); -} - -void Bumper::BeforePhysicsStep(const Timekeeper &timekeeper) { - std::map::iterator it; - - // Clear the forces at the begining of every physics step since brand - // new collision resolutions are being calculated by Box2D each time step - for (it = contact_states_.begin(); it != contact_states_.end(); it++) { - it->second.Reset(); - } -} - -void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { - // The expected behaviour is to always publish non-empty collisions unless - // publish_all_collisions set to false. The publishing of empty collision - // manages the publishing rate of empty collisions when - // publish_all_collisions is true, or it manages the publishing rate all - // empty and non-empty collisions when publish_all_collisions_ is false - if (!publish_all_collisions_ || contact_states_.size() <= 0) { - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - } - - std::map::iterator it; - - flatland_msgs::Collisions collisions; - collisions.header.frame_id = world_frame_id_; - collisions.header.stamp = timekeeper.GetSimTime(); - - // loop through all collisions in our record and publish - for (it = contact_states_.begin(); it != contact_states_.end(); it++) { - b2Contact *c = it->first; - ContactState *s = &it->second; - flatland_msgs::Collision collision; - collision.entity_A = GetModel()->GetName(); - collision.entity_B = s->entity_B->name_; - - collision.body_A = s->body_A->name_; - collision.body_B = s->body_B->name_; - - // If there was no post solve called, which means that the collision - // probably involves a Box2D sensor, therefore there are no contact points, - if (s->num_count > 0) { - b2Manifold *m = c->GetManifold(); - - // go through each collision point - for (int i = 0; i < m->pointCount; i++) { - // calculate average impulse during each time step, the impulse are - // converted to the applied force by dividing the step size - double ave_normal_impulse = s->sum_normal_impulses[i] / s->num_count; - double ave_tangential_impulse = - s->sum_tangential_impulses[i] / s->num_count; - double ave_normal_force = ave_normal_impulse / timekeeper.GetStepSize(); - double ave_tangential_force = - ave_tangential_impulse / timekeeper.GetStepSize(); - - // Calculate the absolute magnitude of forces, forces are not provided - // in vector form because the forces are obtained from averaging - // Box2D impulses which are very inaccurate making them completely - // useless for anything other than ball parking the impact strength - double force_abs = sqrt(ave_normal_force * ave_normal_force + - ave_tangential_force * ave_tangential_force); - - collision.magnitude_forces.push_back(force_abs); - flatland_msgs::Vector2 point; - flatland_msgs::Vector2 normal; - point.x = s->points[i].x; - point.y = s->points[i].y; - normal.x = s->normal.x; - normal.y = s->normal.y; - collision.contact_positions.push_back(point); - collision.contact_normals.push_back(normal); - } - } - - collisions.collisions.push_back(collision); - } - - collisions_publisher_.publish(collisions); -} - -void Bumper::BeginContact(b2Contact *contact) { - Entity *other_entity; - b2Fixture *this_fixture, *other_fixture; - if (!FilterContact(contact, other_entity, this_fixture, other_fixture)) { - return; - } - - // If this is a new contact, add it to the records of alive contacts - if (!contact_states_.count(contact)) { - Body *collision_body = - static_cast(this_fixture->GetBody()->GetUserData()); - - bool ignore = false; - - // check that the body is not in the ignore list - for (unsigned int j = 0; j < excluded_bodies_.size(); j++) { - if (excluded_bodies_[j] == collision_body) { - ignore = true; - break; - } - } - - // add the body to the record of active contacts - if (!ignore) { - contact_states_[contact] = ContactState(); - ContactState *c = &contact_states_[contact]; - c->entity_B = other_entity; - c->body_B = static_cast(other_fixture->GetBody()->GetUserData()); - c->body_A = collision_body; - - // by convention, Box2D normal goes from fixture A to fixture B, the - // sign is used to correct cases when our model isn't fixture A, so that - // normal always points from this fixture to other_fixture - if (contact->GetFixtureA() == this_fixture) { - c->normal_sign = 1; - } else { - c->normal_sign = -1; - } - } - } -} - -void Bumper::EndContact(b2Contact *contact) { - if (!FilterContact(contact)) return; - - // The contact ended, remove it from the list of contacts - if (contact_states_.count(contact)) { - contact_states_.erase(contact); - } else { - // contact is ignored - return; - } -} - -void Bumper::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) { - if (!FilterContact(contact)) return; - - ContactState *state; - if (contact_states_.count(contact)) { - state = &contact_states_[contact]; - } else { - // contact is ignored - return; - } - - // post solve can be called multiple times per time step due to Box2D's - // continuous collision detectopm where Box2D "substeps" in the solver. Each - // substep returns the impulse applied to the body at that substep. We cannot - // obtain the step size of these sub steps, so we just assumed that the - // substeps occurred over evenly subdivided intervals (which is probably - // false), and average all the received impulses later on. The points of - // collision and normals also change slightly at each substep, we simply - // always use ones from the most recent post solve. These results should - // only be used to provide a ball park feel of impact strength - - b2WorldManifold m; - contact->GetWorldManifold(&m); - - state->num_count++; - - // We take data from both contact points even though there might only be - // one point of contact. We sum everything here, and only take the valid - // ones later - state->sum_normal_impulses[0] += impulse->normalImpulses[0]; - state->sum_normal_impulses[1] += impulse->normalImpulses[1]; - state->sum_tangential_impulses[0] += impulse->tangentImpulses[0]; - state->sum_tangential_impulses[1] += impulse->tangentImpulses[1]; - - state->points[0] = m.points[0]; - state->points[1] = m.points[1]; - - state->normal = m.normal; - state->normal *= state->normal_sign; -} -}; - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::Bumper, flatland_server::ModelPlugin) +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name bumper.h + * @brief Bumper plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +Bumper::ContactState::ContactState() { Reset(); } + +void Bumper::ContactState::Reset() { + num_count = 0; + sum_speed = 0.0; +} + +void Bumper::OnInitialize(const YAML::Node &config) { + YamlReader reader(config); + + // defaults + world_frame_id_ = reader.Get("world_frame_id", "map"); + topic_name_ = reader.Get("topic", "collisions"); + publish_all_collisions_ = reader.Get("publish_all_collisions", true); + update_rate_ = reader.Get("update_rate", + std::numeric_limits::infinity()); + + std::vector excluded_body_names = + reader.GetList("exclude", {}, -1, -1); + + reader.EnsureAccessedAllKeys(); + + for (unsigned int i = 0; i < excluded_body_names.size(); i++) { + Body *body = GetModel()->GetBody(excluded_body_names[i]); + + if (body == nullptr) { + throw YAMLException("Body with name \"" + excluded_body_names[i] + + "\" does not exist"); + } else { + excluded_bodies_.push_back(body); + } + } + + update_timer_.SetRate(update_rate_); + collisions_publisher_ = + nh_.advertise(topic_name_, 1); + + ROS_DEBUG_NAMED("Bumper", + "Initialized with params: topic(%s) world_frame_id(%s) " + "publish_all_collisions(%d) update_rate(%f) exclude({%s})", + topic_name_.c_str(), world_frame_id_.c_str(), + publish_all_collisions_, update_rate_, + boost::algorithm::join(excluded_body_names, ",").c_str()); +} + +void Bumper::BeforePhysicsStep(const Timekeeper &timekeeper) { + // Clear the forces at the beginning of every physics step + for (auto &kv : contact_states_) { + kv.second.Reset(); + } +} + +void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { + // The expected behaviour is to always publish non-empty collisions unless + // publish_all_collisions set to false. The publishing of empty collision + // manages the publishing rate of empty collisions when + // publish_all_collisions is true, or it manages the publishing rate all + // empty and non-empty collisions when publish_all_collisions_ is false + if (!publish_all_collisions_ || contact_states_.size() <= 0) { + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + } + + flatland_msgs::Collisions collisions; + collisions.header.frame_id = world_frame_id_; + collisions.header.stamp = timekeeper.GetSimTime(); + + // loop through all collisions in our record and publish + for (auto &kv : contact_states_) { + const ContactState &s = kv.second; + flatland_msgs::Collision collision; + collision.entity_A = GetModel()->GetName(); + collision.entity_B = s.entity_B->name_; + + collision.body_A = s.body_A->name_; + collision.body_B = s.body_B->name_; + + // If there was a hit event, publish the contact force estimate + if (s.num_count > 0) { + double ave_speed = s.sum_speed / s.num_count; + // approachSpeed in m/s; record as a magnitude force proxy + collision.magnitude_forces.push_back(ave_speed); + flatland_msgs::Vector2 point; + flatland_msgs::Vector2 normal; + point.x = s.point.x; + point.y = s.point.y; + normal.x = s.normal.x; + normal.y = s.normal.y; + collision.contact_positions.push_back(point); + collision.contact_normals.push_back(normal); + } + + collisions.collisions.push_back(collision); + } + + collisions_publisher_.publish(collisions); +} + +void Bumper::BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + Entity *other_entity; + b2BodyId this_body, other_body; + if (!FilterContact(shapeIdA, shapeIdB, other_entity, this_body, + other_body)) { + return; + } + + ContactKey key = {shapeIdA, shapeIdB}; + + // If this is a new contact, add it to the records of alive contacts + if (!contact_states_.count(key)) { + Body *collision_body = + static_cast(b2Body_GetUserData(this_body)); + + bool ignore = false; + + // check that the body is not in the ignore list + for (unsigned int j = 0; j < excluded_bodies_.size(); j++) { + if (excluded_bodies_[j] == collision_body) { + ignore = true; + break; + } + } + + // add the body to the record of active contacts + if (!ignore) { + contact_states_[key] = ContactState(); + ContactState *c = &contact_states_[key]; + c->entity_B = other_entity; + c->body_B = static_cast(b2Body_GetUserData(other_body)); + c->body_A = collision_body; + } + } +} + +void Bumper::EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + if (!FilterContact(shapeIdA, shapeIdB)) return; + + ContactKey key = {shapeIdA, shapeIdB}; + if (contact_states_.count(key)) { + contact_states_.erase(key); + } +} + +void Bumper::OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, float approachSpeed) { + if (!FilterContact(shapeIdA, shapeIdB)) return; + + ContactKey key = {shapeIdA, shapeIdB}; + if (!contact_states_.count(key)) return; // ignored contact + + ContactState *state = &contact_states_[key]; + state->num_count++; + state->sum_speed += approachSpeed; + state->point = point; + state->normal = normal; +} +}; + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Bumper, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index 0b17e18b..fdf36a76 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -1,276 +1,290 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name DiffDrive.cpp - * @brief DiffDrive plugin - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_plugins { - -void DiffDrive::TwistCallback(const geometry_msgs::Twist& msg) { - twist_msg_ = msg; -} - -void DiffDrive::OnInitialize(const YAML::Node& config) { - YamlReader reader(config); - enable_odom_pub_ = reader.Get("enable_odom_pub", true); - enable_twist_pub_ = reader.Get("enable_twist_pub", true); - std::string body_name = reader.Get("body"); - std::string odom_frame_id = reader.Get("odom_frame_id", "odom"); - - std::string odom_ns = this->GetModel()->namespace_; - odom_frame_id = this->GetModel()->namespace_ + "_" + odom_frame_id; - - std::string twist_topic = reader.Get("twist_sub", "cmd_vel"); - std::string odom_topic = - reader.Get("odom_pub", "odometry/filtered"); - std::string ground_truth_topic = - reader.Get("ground_truth_pub", "odometry/ground_truth"); - std::string twist_pub_topic = reader.Get("twist_pub", "twist"); - - // noise are in the form of linear x, linear y, angular variances - std::vector odom_twist_noise = - reader.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); - std::vector odom_pose_noise = - reader.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); - - double pub_rate = - reader.Get("pub_rate", std::numeric_limits::infinity()); - update_timer_.SetRate(pub_rate); - - // Angular dynamics constraints - angular_dynamics_.Configure(reader.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node()); - - // Linear dynamics constraints - linear_dynamics_.Configure(reader.SubnodeOpt("linear_dynamics", YamlReader::MAP).Node()); - - // by default the covariance diagonal is the variance of actual noise - // generated, non-diagonal elements are zero assuming the noises are - // independent, we also don't care about linear z, angular x, and angular y - std::array odom_pose_covar_default = {0}; - odom_pose_covar_default[0] = odom_pose_noise[0]; - odom_pose_covar_default[7] = odom_pose_noise[1]; - odom_pose_covar_default[35] = odom_pose_noise[2]; - - std::array odom_twist_covar_default = {0}; - odom_twist_covar_default[0] = odom_twist_noise[0]; - odom_twist_covar_default[7] = odom_twist_noise[1]; - odom_twist_covar_default[35] = odom_twist_noise[2]; - - auto odom_twist_covar = reader.GetArray("odom_twist_covariance", - odom_twist_covar_default); - auto odom_pose_covar = reader.GetArray("odom_pose_covariance", - odom_pose_covar_default); - - reader.EnsureAccessedAllKeys(); - - body_ = GetModel()->GetBody(body_name); - if (body_ == nullptr) { - throw YAMLException("Body with name " + Q(body_name) + " does not exist"); - } - - // publish and subscribe to topics - twist_sub_ = nh_.subscribe(twist_topic, 1, &DiffDrive::TwistCallback, this); - if (enable_odom_pub_) { - odom_pub_ = nh_.advertise(odom_topic, 1); - ground_truth_pub_ = - nh_.advertise(ground_truth_topic, 1); - } - - if (enable_twist_pub_) { - twist_pub_ = nh_.advertise(twist_pub_topic, 1); - } - - // init the values for the messages - ground_truth_msg_.header.frame_id = odom_frame_id; - ground_truth_msg_.child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); - ground_truth_msg_.twist.covariance.fill(0); - ground_truth_msg_.pose.covariance.fill(0); - odom_msg_ = ground_truth_msg_; - - // copy from std::array to boost array - for (unsigned int i = 0; i < 36; i++) { - odom_msg_.twist.covariance[i] = odom_twist_covar[i]; - odom_msg_.pose.covariance[i] = odom_pose_covar[i]; - } - - // init the random number generators - std::random_device rd; - rng_ = std::default_random_engine(rd()); - for (unsigned int i = 0; i < 3; i++) { - // variance is standard deviation squared - noise_gen_[i] = - std::normal_distribution(0.0, sqrt(odom_pose_noise[i])); - } - - for (unsigned int i = 0; i < 3; i++) { - noise_gen_[i + 3] = - std::normal_distribution(0.0, sqrt(odom_twist_noise[i])); - } - - ROS_DEBUG_NAMED("DiffDrive", - "Initialized with params body(%p %s) odom_frame_id(%s) " - "twist_sub(%s) odom_pub(%s) ground_truth_pub(%s) " - "odom_pose_noise({%f,%f,%f}) odom_twist_noise({%f,%f,%f}) " - "pub_rate(%f)\n", - body_, body_->name_.c_str(), odom_frame_id.c_str(), - twist_topic.c_str(), odom_topic.c_str(), - ground_truth_topic.c_str(), odom_pose_noise[0], - odom_pose_noise[1], odom_pose_noise[2], odom_twist_noise[0], - odom_twist_noise[1], odom_twist_noise[2], pub_rate); -} - -void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { - bool publish = update_timer_.CheckUpdate(timekeeper); - - b2Body* b2body = body_->physics_body_; - - b2Vec2 position = b2body->GetPosition(); - float angle = b2body->GetAngle(); - - // Apply dynamics limits - double dt = timekeeper.GetStepSize(); - linear_velocity_ = linear_dynamics_.Limit(linear_velocity_, twist_msg_.linear.x, dt); - angular_velocity_ = angular_dynamics_.Limit(angular_velocity_, twist_msg_.angular.z, dt); - - // we apply the twist velocities, this must be done every physics step to make - // sure Box2D solver applies the correct velocity through out. The velocity - // given in the twist message should be in the local frame - b2Vec2 linear_vel_local(linear_velocity_, 0); - b2Vec2 linear_vel = b2body->GetWorldVector(linear_vel_local); - float angular_vel = angular_velocity_; // angular is independent of frames - - // we want the velocity vector in the world frame at the center of mass - - // V_cm = V_o + W x r_cm/o - // velocity at center of mass equals to the velocity at the body origin plus, - // angular velocity cross product the displacement from the body origin to the - // center of mass - - // r is the vector from body origin to the CM in world frame - b2Vec2 r = b2body->GetWorldCenter() - position; - b2Vec2 linear_vel_cm = linear_vel + angular_vel * b2Vec2(-r.y, r.x); - - b2body->SetLinearVelocity(linear_vel_cm); - b2body->SetAngularVelocity(angular_vel); - - // Update odom+ground truth messages if needed - - if (publish) { - // get the state of the body and publish the data - b2Vec2 linear_vel_local = - b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); - float angular_vel = b2body->GetAngularVelocity(); - - ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); - ground_truth_msg_.pose.pose.position.x = position.x; - ground_truth_msg_.pose.pose.position.y = position.y; - ground_truth_msg_.pose.pose.position.z = 0; - ground_truth_msg_.pose.pose.orientation = - tf::createQuaternionMsgFromYaw(angle); - ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x; - ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y; - ground_truth_msg_.twist.twist.linear.z = 0; - ground_truth_msg_.twist.twist.angular.x = 0; - ground_truth_msg_.twist.twist.angular.y = 0; - ground_truth_msg_.twist.twist.angular.z = angular_vel; - - // add the noise to odom messages - odom_msg_.header.stamp = timekeeper.GetSimTime(); - odom_msg_.pose.pose = ground_truth_msg_.pose.pose; - odom_msg_.twist.twist = ground_truth_msg_.twist.twist; - odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); - odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); - odom_msg_.pose.pose.orientation = - tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); - odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); - odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); - odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); - - if (enable_odom_pub_) { - ground_truth_pub_.publish(ground_truth_msg_); - odom_pub_.publish(odom_msg_); - } - - if (enable_twist_pub_) { - // Transform global frame velocity into local frame to simulate encoder - // readings - geometry_msgs::TwistStamped twist_pub_msg; - twist_pub_msg.header.stamp = timekeeper.GetSimTime(); - twist_pub_msg.header.frame_id = odom_msg_.child_frame_id; - - // Forward velocity in twist.linear.x - twist_pub_msg.twist.linear.x = cos(angle) * linear_vel_local.x + - sin(angle) * linear_vel_local.y + - noise_gen_[3](rng_); - - // Angular velocity in twist.angular.z - twist_pub_msg.twist.angular.z = angular_vel + noise_gen_[5](rng_); - twist_pub_.publish(twist_pub_msg); - } - - // publish odom tf - geometry_msgs::TransformStamped odom_tf; - odom_tf.header = odom_msg_.header; - odom_tf.child_frame_id = odom_msg_.child_frame_id; - odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x; - odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y; - odom_tf.transform.translation.z = 0; - odom_tf.transform.rotation = odom_msg_.pose.pose.orientation; - tf_broadcaster.sendTransform(odom_tf); - } - -} -} - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive, - flatland_server::ModelPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name DiffDrive.cpp + * @brief DiffDrive plugin + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_plugins { + +void DiffDrive::TwistCallback(const geometry_msgs::Twist& msg) { + twist_msg_ = msg; +} + +void DiffDrive::OnInitialize(const YAML::Node& config) { + YamlReader reader(config); + enable_odom_pub_ = reader.Get("enable_odom_pub", true); + enable_odom_tf_pub_ = reader.Get("enable_odom_tf_pub", true); + enable_twist_pub_ = reader.Get("enable_twist_pub", true); + enable_ground_truth_pub_ = reader.Get("enable_ground_truth_pub", true); + twist_in_local_frame_ = reader.Get("twist_in_local_frame", true); + std::string body_name = reader.Get("body"); + std::string odom_frame_id = reader.Get("odom_frame_id", "odom"); + + std::string odom_ns = this->GetModel()->namespace_; + odom_frame_id = this->GetModel()->namespace_ + "_" + odom_frame_id; + + std::string twist_topic = reader.Get("twist_sub", "cmd_vel"); + std::string odom_topic = + reader.Get("odom_pub", "odometry/filtered"); + std::string ground_truth_topic = + reader.Get("ground_truth_pub", "odometry/ground_truth"); + std::string twist_pub_topic = reader.Get("twist_pub", "twist"); + + // noise are in the form of linear x, linear y, angular variances + std::vector odom_twist_noise = + reader.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); + std::vector odom_pose_noise = + reader.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); + + double pub_rate = + reader.Get("pub_rate", std::numeric_limits::infinity()); + update_timer_.SetRate(pub_rate); + + // Angular dynamics constraints + angular_dynamics_.Configure(reader.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node()); + + // Linear dynamics constraints + linear_dynamics_.Configure(reader.SubnodeOpt("linear_dynamics", YamlReader::MAP).Node()); + + // by default the covariance diagonal is the variance of actual noise + // generated, non-diagonal elements are zero assuming the noises are + // independent, we also don't care about linear z, angular x, and angular y + std::array odom_pose_covar_default = {0}; + odom_pose_covar_default[0] = odom_pose_noise[0]; + odom_pose_covar_default[7] = odom_pose_noise[1]; + odom_pose_covar_default[35] = odom_pose_noise[2]; + + std::array odom_twist_covar_default = {0}; + odom_twist_covar_default[0] = odom_twist_noise[0]; + odom_twist_covar_default[7] = odom_twist_noise[1]; + odom_twist_covar_default[35] = odom_twist_noise[2]; + + auto odom_twist_covar = reader.GetArray("odom_twist_covariance", + odom_twist_covar_default); + auto odom_pose_covar = reader.GetArray("odom_pose_covariance", + odom_pose_covar_default); + + reader.EnsureAccessedAllKeys(); + + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name " + Q(body_name) + " does not exist"); + } + + // publish and subscribe to topics + twist_sub_ = nh_.subscribe(twist_topic, 1, &DiffDrive::TwistCallback, this); + if (enable_odom_pub_) { + odom_pub_ = nh_.advertise(odom_topic, 1); + } + + if (enable_ground_truth_pub_) { + ground_truth_pub_ = + nh_.advertise(ground_truth_topic, 1); + } + + if (enable_twist_pub_) { + twist_pub_ = nh_.advertise(twist_pub_topic, 1); + } + + // init the values for the messages + ground_truth_msg_.header.frame_id = odom_frame_id; + ground_truth_msg_.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); + ground_truth_msg_.twist.covariance.fill(0); + ground_truth_msg_.pose.covariance.fill(0); + odom_msg_ = ground_truth_msg_; + + // copy from std::array to boost array + for (unsigned int i = 0; i < 36; i++) { + odom_msg_.twist.covariance[i] = odom_twist_covar[i]; + odom_msg_.pose.covariance[i] = odom_pose_covar[i]; + } + + // init the random number generators + std::random_device rd; + rng_ = std::default_random_engine(rd()); + for (unsigned int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i] = + std::normal_distribution(0.0, sqrt(odom_pose_noise[i])); + } + + for (unsigned int i = 0; i < 3; i++) { + noise_gen_[i + 3] = + std::normal_distribution(0.0, sqrt(odom_twist_noise[i])); + } + + ROS_DEBUG_NAMED("DiffDrive", + "Initialized with params body(%p %s) odom_frame_id(%s) " + "twist_sub(%s) odom_pub(%s) ground_truth_pub(%s) " + "odom_pose_noise({%f,%f,%f}) odom_twist_noise({%f,%f,%f}) " + "pub_rate(%f)\n", + body_, body_->name_.c_str(), odom_frame_id.c_str(), + twist_topic.c_str(), odom_topic.c_str(), + ground_truth_topic.c_str(), odom_pose_noise[0], + odom_pose_noise[1], odom_pose_noise[2], odom_twist_noise[0], + odom_twist_noise[1], odom_twist_noise[2], pub_rate); +} + +void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { + bool publish = update_timer_.CheckUpdate(timekeeper); + + b2BodyId b2body = body_->physics_body_; + + b2Vec2 position = b2Body_GetPosition(b2body); + float angle = b2Rot_GetAngle(b2Body_GetRotation(b2body)); + + // Apply dynamics limits + double dt = timekeeper.GetStepSize(); + linear_velocity_ = linear_dynamics_.Limit(linear_velocity_, twist_msg_.linear.x, dt); + angular_velocity_ = angular_dynamics_.Limit(angular_velocity_, twist_msg_.angular.z, dt); + + // we apply the twist velocities, this must be done every physics step to make + // sure Box2D solver applies the correct velocity through out. The velocity + // given in the twist message should be in the local frame + b2Vec2 linear_vel_local = {static_cast(linear_velocity_), 0.0f}; + b2Vec2 linear_vel = b2Body_GetWorldVector(b2body, linear_vel_local); + float angular_vel = static_cast(angular_velocity_); + + // we want the velocity vector in the world frame at the center of mass + + // V_cm = V_o + W x r_cm/o + // velocity at center of mass equals to the velocity at the body origin plus, + // angular velocity cross product the displacement from the body origin to the + // center of mass + + // r is the vector from body origin to the CM in world frame + b2Vec2 com = b2Body_GetLocalCenterOfMass(b2body); + b2Vec2 wcom = b2Body_GetWorldPoint(b2body, com); + b2Vec2 r = {wcom.x - position.x, wcom.y - position.y}; + b2Vec2 linear_vel_cm = {linear_vel.x + angular_vel * (-r.y), + linear_vel.y + angular_vel * r.x}; + + b2Body_SetLinearVelocity(b2body, linear_vel_cm); + b2Body_SetAngularVelocity(b2body, angular_vel); + + // Update odom+ground truth messages if needed + + if (publish) { + // get the state of the body and publish the data + b2Vec2 local_vel_world = b2Body_GetLinearVelocity(b2body); + b2Vec2 linear_vel_local_meas = b2Body_GetLocalVector(b2body, local_vel_world); + float angular_vel = b2Body_GetAngularVelocity(b2body); + + ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); + ground_truth_msg_.pose.pose.position.x = position.x; + ground_truth_msg_.pose.pose.position.y = position.y; + ground_truth_msg_.pose.pose.position.z = 0; + ground_truth_msg_.pose.pose.orientation = + tf::createQuaternionMsgFromYaw(angle); + ground_truth_msg_.twist.twist.linear.x = linear_vel_local_meas.x; + ground_truth_msg_.twist.twist.linear.y = linear_vel_local_meas.y; + ground_truth_msg_.twist.twist.linear.z = 0; + ground_truth_msg_.twist.twist.angular.x = 0; + ground_truth_msg_.twist.twist.angular.y = 0; + ground_truth_msg_.twist.twist.angular.z = angular_vel; + + // add the noise to odom messages + odom_msg_.header.stamp = timekeeper.GetSimTime(); + odom_msg_.pose.pose = ground_truth_msg_.pose.pose; + odom_msg_.twist.twist = ground_truth_msg_.twist.twist; + odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); + odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); + odom_msg_.pose.pose.orientation = + tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); + odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); + odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); + odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); + + if (enable_ground_truth_pub_) { + ground_truth_pub_.publish(ground_truth_msg_); + } + + if (enable_odom_pub_) { + odom_pub_.publish(odom_msg_); + } + + if (enable_twist_pub_) { + // Transform global frame velocity into local frame to simulate encoder + // readings + geometry_msgs::TwistStamped twist_pub_msg; + twist_pub_msg.header.stamp = timekeeper.GetSimTime(); + twist_pub_msg.header.frame_id = odom_msg_.child_frame_id; + + // Forward velocity in twist.linear.x + twist_pub_msg.twist.linear.x = cos(angle) * linear_vel_local_meas.x + + sin(angle) * linear_vel_local_meas.y + + noise_gen_[3](rng_); + + // Angular velocity in twist.angular.z + twist_pub_msg.twist.angular.z = angular_vel + noise_gen_[5](rng_); + twist_pub_.publish(twist_pub_msg); + } + + // publish odom tf + if (enable_odom_tf_pub_) { + geometry_msgs::TransformStamped odom_tf; + odom_tf.header = odom_msg_.header; + odom_tf.child_frame_id = odom_msg_.child_frame_id; + odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x; + odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y; + odom_tf.transform.translation.z = 0; + odom_tf.transform.rotation = odom_msg_.pose.pose.orientation; + tf_broadcaster.sendTransform(odom_tf); + } + } + +} +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive, + flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/dynamics_limits.cpp b/flatland_plugins/src/dynamics_limits.cpp index e9744d91..5a7f20b0 100644 --- a/flatland_plugins/src/dynamics_limits.cpp +++ b/flatland_plugins/src/dynamics_limits.cpp @@ -1,124 +1,124 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2021 Avidbots Corp. - * @name dynamics_limits.cpp - * @brief A generic acceleration, deceleration and velocity bounding utility class - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_plugins/dynamics_limits.h" - -namespace flatland_plugins { - -void DynamicsLimits::Configure(const YAML::Node &config) { - flatland_server::YamlReader reader(config); - acceleration_limit_ = reader.Get("acceleration_limit", 0.0); // 0.0 default disables the limit - deceleration_limit_ = reader.Get("deceleration_limit", std::numeric_limits::infinity()); - - // if no acceleration limit is set, default it to equal the acceleration limit - if (deceleration_limit_ == std::numeric_limits::infinity()) { - deceleration_limit_ = acceleration_limit_; - } - - velocity_limit_ = reader.Get("velocity_limit", 0.0); // 0.0 default disables the limit -} - -double DynamicsLimits::Saturate(double in, double lower, double upper) { - if (lower > upper) { - return in; - } - double out = in; - out = std::max(out, lower); - out = std::min(out, upper); - return out; -} - -double DynamicsLimits::Limit(double velocity, double target_velocity, double timestep) { - - // if enabled, apply velocity limit - if (velocity_limit_ != 0.0) { - // Saturating the command also saturates the steering velocity - target_velocity = DynamicsLimits::Saturate(target_velocity, -velocity_limit_, velocity_limit_); - } - - // if the target velocity magnitude is larger, and in the same direction, we're accelerating - double acceleration_limit; - if (target_velocity == 0) { // we can only be decellerating - acceleration_limit = deceleration_limit_; - } else if (velocity == 0) { // we can only be accelerating - acceleration_limit = acceleration_limit_; - } else if (velocity*target_velocity < 0) { // velocities are in different directions, we must be decelerating at least initially - if (deceleration_limit_ != 0.0) { - double initial_deceleration = DynamicsLimits::Saturate((target_velocity - velocity) / timestep, -deceleration_limit_, deceleration_limit_); - double new_velocity = velocity + initial_deceleration * timestep; - if (new_velocity*velocity>0 && acceleration_limit_ != 0.0) { // no zero velocity crossing, we're only decelerating - acceleration_limit = deceleration_limit_; - } else { // We decelerate through a zero velocity crossing, both limits apply proportionally - double deceleration_time = fabs(velocity)/deceleration_limit_; - if (acceleration_limit_ == 0) { // odd corner case where there's a deceleration limit but no acceleration limit - acceleration_limit = 0; // effectively no limit - } else { // we have both acceleration and deceleration limits, which apply proportionally - acceleration_limit = deceleration_limit_ * deceleration_time/timestep + acceleration_limit_ * (1-deceleration_time/timestep); - } - } - - } else { - velocity = 0; // override incoming velocity, since we have no deceleration limit - acceleration_limit = acceleration_limit_; - } - } else if (fabs(velocity) < fabs(target_velocity)) { // new velocity magnitude is larger than old, and in the same direction: accelerating - acceleration_limit = acceleration_limit_; - } else { // new velocity magnitude is smaller or equal than old, in the same direction: decelerating, unless there's a zero crossing inside the timestep - acceleration_limit = deceleration_limit_; - } - - // compute accleration, and limit it if needed - double acceleration = (target_velocity - velocity) / timestep; - if (acceleration_limit != 0.0) { - acceleration = - DynamicsLimits::Saturate(acceleration, -acceleration_limit, acceleration_limit); - } - - // (2) Update the new steering velocity - return velocity + acceleration * timestep; -} - -} /* end namespace flatland_plugins */ \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2021 Avidbots Corp. + * @name dynamics_limits.cpp + * @brief A generic acceleration, deceleration and velocity bounding utility class + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_plugins/dynamics_limits.h" + +namespace flatland_plugins { + +void DynamicsLimits::Configure(const YAML::Node &config) { + flatland_server::YamlReader reader(config); + acceleration_limit_ = reader.Get("acceleration_limit", 0.0); // 0.0 default disables the limit + deceleration_limit_ = reader.Get("deceleration_limit", std::numeric_limits::infinity()); + + // if no acceleration limit is set, default it to equal the acceleration limit + if (deceleration_limit_ == std::numeric_limits::infinity()) { + deceleration_limit_ = acceleration_limit_; + } + + velocity_limit_ = reader.Get("velocity_limit", 0.0); // 0.0 default disables the limit +} + +double DynamicsLimits::Saturate(double in, double lower, double upper) { + if (lower > upper) { + return in; + } + double out = in; + out = std::max(out, lower); + out = std::min(out, upper); + return out; +} + +double DynamicsLimits::Limit(double velocity, double target_velocity, double timestep) { + + // if enabled, apply velocity limit + if (velocity_limit_ != 0.0) { + // Saturating the command also saturates the steering velocity + target_velocity = DynamicsLimits::Saturate(target_velocity, -velocity_limit_, velocity_limit_); + } + + // if the target velocity magnitude is larger, and in the same direction, we're accelerating + double acceleration_limit; + if (target_velocity == 0) { // we can only be decellerating + acceleration_limit = deceleration_limit_; + } else if (velocity == 0) { // we can only be accelerating + acceleration_limit = acceleration_limit_; + } else if (velocity*target_velocity < 0) { // velocities are in different directions, we must be decelerating at least initially + if (deceleration_limit_ != 0.0) { + double initial_deceleration = DynamicsLimits::Saturate((target_velocity - velocity) / timestep, -deceleration_limit_, deceleration_limit_); + double new_velocity = velocity + initial_deceleration * timestep; + if (new_velocity*velocity>0 && acceleration_limit_ != 0.0) { // no zero velocity crossing, we're only decelerating + acceleration_limit = deceleration_limit_; + } else { // We decelerate through a zero velocity crossing, both limits apply proportionally + double deceleration_time = fabs(velocity)/deceleration_limit_; + if (acceleration_limit_ == 0) { // odd corner case where there's a deceleration limit but no acceleration limit + acceleration_limit = 0; // effectively no limit + } else { // we have both acceleration and deceleration limits, which apply proportionally + acceleration_limit = deceleration_limit_ * deceleration_time/timestep + acceleration_limit_ * (1-deceleration_time/timestep); + } + } + + } else { + velocity = 0; // override incoming velocity, since we have no deceleration limit + acceleration_limit = acceleration_limit_; + } + } else if (fabs(velocity) < fabs(target_velocity)) { // new velocity magnitude is larger than old, and in the same direction: accelerating + acceleration_limit = acceleration_limit_; + } else { // new velocity magnitude is smaller or equal than old, in the same direction: decelerating, unless there's a zero crossing inside the timestep + acceleration_limit = deceleration_limit_; + } + + // compute accleration, and limit it if needed + double acceleration = (target_velocity - velocity) / timestep; + if (acceleration_limit != 0.0) { + acceleration = + DynamicsLimits::Saturate(acceleration, -acceleration_limit, acceleration_limit); + } + + // (2) Update the new steering velocity + return velocity + acceleration * timestep; +} + +} /* end namespace flatland_plugins */ diff --git a/flatland_plugins/src/gps.cpp b/flatland_plugins/src/gps.cpp index f17498f3..de1bc9a4 100644 --- a/flatland_plugins/src/gps.cpp +++ b/flatland_plugins/src/gps.cpp @@ -1,128 +1,128 @@ -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -double Gps::WGS84_A = 6378137.0; -double Gps::WGS84_E2 = 0.0066943799831668; - -void Gps::OnInitialize(const YAML::Node &config) { - ParseParameters(config); - update_timer_.SetRate(update_rate_); - fix_publisher_ = nh_.advertise(topic_, 1); - - double c = cos(origin_.theta); - double s = sin(origin_.theta); - double x = origin_.x, y = origin_.y; - m_body_to_gps_ << c, -s, x, s, c, y, 0, 0, 1; -} - -void Gps::BeforePhysicsStep(const Timekeeper &timekeeper) { - // keep the update rate - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - - // only compute and publish when the number of subscribers is not zero - if (fix_publisher_.getNumSubscribers() > 0) { - UpdateFix(); - gps_fix_.header.stamp = timekeeper.GetSimTime(); - fix_publisher_.publish(gps_fix_); - } - - if (broadcast_tf_) { - gps_tf_.header.stamp = timekeeper.GetSimTime(); - tf_broadcaster_.sendTransform(gps_tf_); - } -} - -void Gps::ComputeReferenceEcef() { - double s_lat = sin(ref_lat_rad_); - double c_lat = cos(ref_lat_rad_); - double s_lon = sin(ref_lon_rad_); - double c_lon = cos(ref_lon_rad_); - - double n = WGS84_A / sqrt(1.0 - WGS84_E2 * s_lat * s_lat); - - ref_ecef_x_ = n * c_lat * c_lon; - ref_ecef_y_ = n * c_lat * s_lon; - ref_ecef_z_ = n * (1.0 - WGS84_E2) * s_lat; -} - -void Gps::UpdateFix() { - const b2Transform &t = body_->GetPhysicsBody()->GetTransform(); - Eigen::Matrix3f m_world_to_body; - m_world_to_body << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; - Eigen::Matrix3f m_world_to_gps = m_world_to_body * m_body_to_gps_; - b2Vec2 gps_pos(m_world_to_gps(0, 2), m_world_to_gps(1, 2)); - - /* Convert simulation position into ECEF coordinates */ - double s_lat = sin(ref_lat_rad_); - double c_lat = cos(ref_lat_rad_); - double s_lon = sin(ref_lon_rad_); - double c_lon = cos(ref_lon_rad_); - - double ecef_x = ref_ecef_x_ - s_lon * gps_pos.x - s_lat * c_lon * gps_pos.y; - double ecef_y = ref_ecef_y_ + c_lon * gps_pos.x - s_lat * s_lon * gps_pos.y; - double ecef_z = ref_ecef_z_ + c_lat * gps_pos.y; - - /* Convert ECEF to lat/lon */ - // Longitude is easy - gps_fix_.longitude = atan2(ecef_y, ecef_x) * 180.0 * M_1_PI; - - // Iterative solution for latitude - double r; - double alt; - double p = sqrt(ecef_x * ecef_x + ecef_y * ecef_y); - double lat_rad = atan(p / ecef_z); - for (unsigned int i = 0; i < 4; i++) { - double s_lat = sin(lat_rad); - r = WGS84_A / sqrt(1.0 - WGS84_E2 * s_lat * s_lat); - alt = p / cos(lat_rad) - r; - lat_rad = atan(ecef_z / p / (1 - WGS84_E2 * r / (r + alt))); - } - gps_fix_.latitude = lat_rad * 180.0 * M_1_PI; - gps_fix_.altitude = 0.0; -} - -void Gps::ParseParameters(const YAML::Node &config) { - YamlReader reader(config); - std::string body_name = reader.Get("body"); - topic_ = reader.Get("topic", "gps/fix"); - frame_id_ = reader.Get("frame", GetName()); - broadcast_tf_ = reader.Get("broadcast_tf", true); - update_rate_ = reader.Get("update_rate", 10.0); - ref_lat_rad_ = M_PI / 180.0 * reader.Get("ref_lat", 0.0); - ref_lon_rad_ = M_PI / 180.0 * reader.Get("ref_lon", 0.0); - ComputeReferenceEcef(); - origin_ = reader.GetPose("origin", Pose(0, 0, 0)); - - body_ = GetModel()->GetBody(body_name); - if (!body_) { - throw YAMLException("Cannot find body with name " + body_name); - } - - std::string parent_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(body_->GetName())); - std::string child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); - - // Set constant frame ID in GPS fix message - gps_fix_.header.frame_id = child_frame_id; - - // Construct constant TF transform - gps_tf_.header.frame_id = parent_frame_id; - gps_tf_.child_frame_id = child_frame_id; - gps_tf_.transform.translation.x = origin_.x; - gps_tf_.transform.translation.y = origin_.y; - gps_tf_.transform.translation.z = 0.0; - gps_tf_.transform.rotation.x = 0.0; - gps_tf_.transform.rotation.y = 0.0; - gps_tf_.transform.rotation.z = sin(0.5 * origin_.theta); - gps_tf_.transform.rotation.w = cos(0.5 * origin_.theta); -} -} - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::Gps, flatland_server::ModelPlugin) +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +double Gps::WGS84_A = 6378137.0; +double Gps::WGS84_E2 = 0.0066943799831668; + +void Gps::OnInitialize(const YAML::Node &config) { + ParseParameters(config); + update_timer_.SetRate(update_rate_); + fix_publisher_ = nh_.advertise(topic_, 1); + + double c = cos(origin_.theta); + double s = sin(origin_.theta); + double x = origin_.x, y = origin_.y; + m_body_to_gps_ << c, -s, x, s, c, y, 0, 0, 1; +} + +void Gps::BeforePhysicsStep(const Timekeeper &timekeeper) { + // keep the update rate + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + + // only compute and publish when the number of subscribers is not zero + if (fix_publisher_.getNumSubscribers() > 0) { + UpdateFix(); + gps_fix_.header.stamp = timekeeper.GetSimTime(); + fix_publisher_.publish(gps_fix_); + } + + if (broadcast_tf_) { + gps_tf_.header.stamp = timekeeper.GetSimTime(); + tf_broadcaster_.sendTransform(gps_tf_); + } +} + +void Gps::ComputeReferenceEcef() { + double s_lat = sin(ref_lat_rad_); + double c_lat = cos(ref_lat_rad_); + double s_lon = sin(ref_lon_rad_); + double c_lon = cos(ref_lon_rad_); + + double n = WGS84_A / sqrt(1.0 - WGS84_E2 * s_lat * s_lat); + + ref_ecef_x_ = n * c_lat * c_lon; + ref_ecef_y_ = n * c_lat * s_lon; + ref_ecef_z_ = n * (1.0 - WGS84_E2) * s_lat; +} + +void Gps::UpdateFix() { + b2Transform t = b2Body_GetTransform(body_->GetPhysicsBody()); + Eigen::Matrix3f m_world_to_body; + m_world_to_body << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; + Eigen::Matrix3f m_world_to_gps = m_world_to_body * m_body_to_gps_; + b2Vec2 gps_pos = {(float)m_world_to_gps(0, 2), (float)m_world_to_gps(1, 2)}; + + /* Convert simulation position into ECEF coordinates */ + double s_lat = sin(ref_lat_rad_); + double c_lat = cos(ref_lat_rad_); + double s_lon = sin(ref_lon_rad_); + double c_lon = cos(ref_lon_rad_); + + double ecef_x = ref_ecef_x_ - s_lon * gps_pos.x - s_lat * c_lon * gps_pos.y; + double ecef_y = ref_ecef_y_ + c_lon * gps_pos.x - s_lat * s_lon * gps_pos.y; + double ecef_z = ref_ecef_z_ + c_lat * gps_pos.y; + + /* Convert ECEF to lat/lon */ + // Longitude is easy + gps_fix_.longitude = atan2(ecef_y, ecef_x) * 180.0 * M_1_PI; + + // Iterative solution for latitude + double r; + double alt; + double p = sqrt(ecef_x * ecef_x + ecef_y * ecef_y); + double lat_rad = atan(p / ecef_z); + for (unsigned int i = 0; i < 4; i++) { + double s_lat = sin(lat_rad); + r = WGS84_A / sqrt(1.0 - WGS84_E2 * s_lat * s_lat); + alt = p / cos(lat_rad) - r; + lat_rad = atan(ecef_z / p / (1 - WGS84_E2 * r / (r + alt))); + } + gps_fix_.latitude = lat_rad * 180.0 * M_1_PI; + gps_fix_.altitude = 0.0; +} + +void Gps::ParseParameters(const YAML::Node &config) { + YamlReader reader(config); + std::string body_name = reader.Get("body"); + topic_ = reader.Get("topic", "gps/fix"); + frame_id_ = reader.Get("frame", GetName()); + broadcast_tf_ = reader.Get("broadcast_tf", true); + update_rate_ = reader.Get("update_rate", 10.0); + ref_lat_rad_ = M_PI / 180.0 * reader.Get("ref_lat", 0.0); + ref_lon_rad_ = M_PI / 180.0 * reader.Get("ref_lon", 0.0); + ComputeReferenceEcef(); + origin_ = reader.GetPose("origin", Pose(0, 0, 0)); + + body_ = GetModel()->GetBody(body_name); + if (!body_) { + throw YAMLException("Cannot find body with name " + body_name); + } + + std::string parent_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(body_->GetName())); + std::string child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); + + // Set constant frame ID in GPS fix message + gps_fix_.header.frame_id = child_frame_id; + + // Construct constant TF transform + gps_tf_.header.frame_id = parent_frame_id; + gps_tf_.child_frame_id = child_frame_id; + gps_tf_.transform.translation.x = origin_.x; + gps_tf_.transform.translation.y = origin_.y; + gps_tf_.transform.translation.z = 0.0; + gps_tf_.transform.rotation.x = 0.0; + gps_tf_.transform.rotation.y = 0.0; + gps_tf_.transform.rotation.z = sin(0.5 * origin_.theta); + gps_tf_.transform.rotation.w = cos(0.5 * origin_.theta); +} +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Gps, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/imu.cpp b/flatland_plugins/src/imu.cpp new file mode 100644 index 00000000..75cb994b --- /dev/null +++ b/flatland_plugins/src/imu.cpp @@ -0,0 +1,239 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name Imu.cpp + * @brief Imu plugin + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_plugins { + +void Imu::OnInitialize(const YAML::Node& config) { + YamlReader reader(config); + enable_imu_pub_ = reader.Get("enable_imu_pub", true); + + std::string body_name = reader.Get("body"); + imu_frame_id_ = reader.Get("imu_frame_id", "imu"); + + std::string imu_topic = reader.Get("imu_pub", "imu/filtered"); + std::string ground_truth_topic = + reader.Get("ground_truth_pub", "imu/ground_truth"); + + // noise are in the form of linear x, linear y, angular variances + std::vector orientation_noise = + reader.GetList("orientation_noise", {0, 0, 0}, 3, 3); + std::vector angular_velocity_noise = + reader.GetList("angular_velocity_noise", {0, 0, 0}, 3, 3); + std::vector linear_acceleration_noise = + reader.GetList("linear_acceleration_noise", {0, 0, 0}, 3, 3); + + pub_rate_ = + reader.Get("pub_rate", std::numeric_limits::infinity()); + update_timer_.SetRate(pub_rate_); + + broadcast_tf_ = reader.Get("broadcast_tf", true); + + // by default the covariance diagonal is the variance of actual noise + // generated, non-diagonal elements are zero assuming the noises are + // independent, we also don't care about linear z, angular x, and angular y + std::array orientation_covar_default = {0}; + orientation_covar_default[0] = orientation_noise[0]; + orientation_covar_default[4] = orientation_noise[1]; + orientation_covar_default[8] = orientation_noise[2]; + + std::array angular_velocity_covar_default = {0}; + angular_velocity_covar_default[0] = angular_velocity_noise[0]; + angular_velocity_covar_default[4] = angular_velocity_noise[1]; + angular_velocity_covar_default[8] = angular_velocity_noise[2]; + + std::array linear_acceleration_covar_default = {0}; + linear_acceleration_covar_default[0] = linear_acceleration_noise[0]; + linear_acceleration_covar_default[4] = linear_acceleration_noise[1]; + linear_acceleration_covar_default[8] = linear_acceleration_noise[2]; + + auto orientation_covar = reader.GetArray( + "orientation_covariance", orientation_covar_default); + + auto angular_velocity_covar = reader.GetArray( + "angular_velocity_covariance", angular_velocity_covar_default); + + auto linear_acceleration_covar = reader.GetArray( + "linear_acceleration_covariance", linear_acceleration_covar_default); + + reader.EnsureAccessedAllKeys(); + + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name " + Q(body_name) + " does not exist"); + } + + imu_pub_ = nh_.advertise(imu_topic, 1); + ground_truth_pub_ = nh_.advertise(ground_truth_topic, 1); + + // init the values for the messages + ground_truth_msg_.header.frame_id = imu_frame_id_; + tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); + ground_truth_msg_.orientation_covariance.fill(0); + ground_truth_msg_.angular_velocity_covariance.fill(0); + ground_truth_msg_.linear_acceleration_covariance.fill(0); + imu_msg_ = ground_truth_msg_; + + // copy from std::array to boost array + for (int i = 0; i < 9; i++) { + imu_msg_.orientation_covariance[i] = orientation_covar[i]; + imu_msg_.angular_velocity_covariance[i] = angular_velocity_covar[i]; + imu_msg_.linear_acceleration_covariance[i] = linear_acceleration_covar[i]; + } + + // init the random number generators + std::random_device rd; + rng_ = std::default_random_engine(rd()); + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i] = + std::normal_distribution(0.0, sqrt(orientation_noise[i])); + } + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i + 3] = + std::normal_distribution(0.0, sqrt(angular_velocity_noise[i])); + } + for (int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i + 6] = std::normal_distribution( + 0.0, sqrt(linear_acceleration_noise[i])); + } + + imu_tf_.header.frame_id = tf::resolve( + "", GetModel()->NameSpaceTF(body_->GetName())); // Todo: parent_tf param + imu_tf_.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(imu_frame_id_)); + imu_tf_.transform.translation.x = 0; // origin_.x; TODO: read position + imu_tf_.transform.translation.y = 0; // origin_.y; + imu_tf_.transform.translation.z = 0; + imu_tf_.transform.rotation.x = 0; // q.x(); + imu_tf_.transform.rotation.y = 0; // q.y(); + imu_tf_.transform.rotation.z = 0; // q.z(); + imu_tf_.transform.rotation.w = 1; // q.w(); + + ROS_DEBUG_NAMED( + "Imu", + "Initialized with params body(%p %s) imu_frame_id(%s) " + "imu_pub(%s) ground_truth_pub(%s) " + "orientation_noise({%f,%f,%f}) angular_velocity_noise({%f,%f,%f}) " + "linear_acceleration_velocity({%f,%f,%f}) " + "pub_rate(%f)\n", + body_, body_->name_.c_str(), imu_frame_id_.c_str(), imu_topic.c_str(), + ground_truth_topic.c_str(), orientation_noise[0], orientation_noise[1], + orientation_noise[2], angular_velocity_noise[0], + angular_velocity_noise[1], angular_velocity_noise[2], + linear_acceleration_noise[0], linear_acceleration_noise[1], + linear_acceleration_noise[2], pub_rate_); +} + +void Imu::AfterPhysicsStep(const Timekeeper& timekeeper) { + bool publish = update_timer_.CheckUpdate(timekeeper); + + b2Body* b2body = body_->physics_body_; + + b2Vec2 position = b2body->GetPosition(); + float angle = b2body->GetAngle(); + b2Vec2 linear_vel_local = + b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); + float angular_vel = b2body->GetAngularVelocity(); + + if (publish) { + // get the state of the body and publish the data + + ground_truth_msg_.header.stamp = ros::Time::now(); + ground_truth_msg_.orientation = tf::createQuaternionMsgFromYaw(angle); + ground_truth_msg_.angular_velocity.z = angular_vel; + + double global_acceleration_x = + (linear_vel_local.x - linear_vel_local_prev.x) * pub_rate_; + double global_acceleration_y = + (linear_vel_local.y - linear_vel_local_prev.y) * pub_rate_; + + ground_truth_msg_.linear_acceleration.x = + cos(angle) * global_acceleration_x + sin(angle) * global_acceleration_y; + ground_truth_msg_.linear_acceleration.y = + sin(angle) * global_acceleration_x + cos(angle) * global_acceleration_y; + + // ROS_INFO_STREAM_THROTTLE( + // 1, "" << linear_vel_local.x << " " << linear_vel_local_prev.x << " " + //<< pub_rate_); + // add the noise to odom messages + imu_msg_.header.stamp = ros::Time::now(); + + imu_msg_.orientation = + tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); + + imu_msg_.angular_velocity = ground_truth_msg_.angular_velocity; + imu_msg_.angular_velocity.z += noise_gen_[5](rng_); + + imu_msg_.linear_acceleration = ground_truth_msg_.linear_acceleration; + imu_msg_.linear_acceleration.x += noise_gen_[6](rng_); + imu_msg_.linear_acceleration.y += noise_gen_[7](rng_); + + if (enable_imu_pub_) { + ground_truth_pub_.publish(ground_truth_msg_); + imu_pub_.publish(imu_msg_); + } + linear_vel_local_prev = linear_vel_local; + } + + if (broadcast_tf_) { + imu_tf_.header.stamp = ros::Time::now(); + tf_broadcaster_.sendTransform(imu_tf_); + } +} +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Imu, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index 318d3133..cc4fe770 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -1,282 +1,277 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name laser.cpp - * @brief Laser plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -void Laser::OnInitialize(const YAML::Node &config) { - ParseParameters(config); - - update_timer_.SetRate(update_rate_); - scan_publisher_ = nh_.advertise(topic_, 1); - - // construct the body to laser transformation matrix once since it never - // changes - double c = cos(origin_.theta); - double s = sin(origin_.theta); - double x = origin_.x, y = origin_.y; - m_body_to_laser_ << c, -s, x, s, c, y, 0, 0, 1; - - unsigned int num_laser_points = - std::lround((max_angle_ - min_angle_) / increment_) + 1; - - // initialize size for the matrix storing the laser points - m_laser_points_ = Eigen::MatrixXf(3, num_laser_points); - m_world_laser_points_ = Eigen::MatrixXf(3, num_laser_points); - v_zero_point_ << 0, 0, 1; - - // pre-calculate the laser points w.r.t to the laser frame, since this never - // changes - for (unsigned int i = 0; i < num_laser_points; i++) { - float angle = min_angle_ + i * increment_; - - float x = range_ * cos(angle); - float y = range_ * sin(angle); - - m_laser_points_(0, i) = x; - m_laser_points_(1, i) = y; - m_laser_points_(2, i) = 1; - } - - // initialize constants in the laser scan message - laser_scan_.angle_min = min_angle_; - laser_scan_.angle_max = max_angle_; - laser_scan_.angle_increment = increment_; - laser_scan_.time_increment = 0; - laser_scan_.scan_time = 0; - laser_scan_.range_min = 0; - laser_scan_.range_max = range_; - laser_scan_.ranges.resize(num_laser_points); - if (reflectance_layers_bits_) - laser_scan_.intensities.resize(num_laser_points); - else - laser_scan_.intensities.resize(0); - laser_scan_.header.seq = 0; - laser_scan_.header.frame_id = - tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); - - // Broadcast transform between the body and laser - tf::Quaternion q; - q.setRPY(0, 0, origin_.theta); - - laser_tf_.header.frame_id = tf::resolve( - "", GetModel()->NameSpaceTF(body_->GetName())); // Todo: parent_tf param - laser_tf_.child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); - laser_tf_.transform.translation.x = origin_.x; - laser_tf_.transform.translation.y = origin_.y; - laser_tf_.transform.translation.z = 0; - laser_tf_.transform.rotation.x = q.x(); - laser_tf_.transform.rotation.y = q.y(); - laser_tf_.transform.rotation.z = q.z(); - laser_tf_.transform.rotation.w = q.w(); -} - -void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { - // keep the update rate - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - - // only compute and publish when the number of subscribers is not zero - if (scan_publisher_.getNumSubscribers() > 0) { - ComputeLaserRanges(); - laser_scan_.header.stamp = timekeeper.GetSimTime(); - scan_publisher_.publish(laser_scan_); - } - - if (broadcast_tf_) { - laser_tf_.header.stamp = timekeeper.GetSimTime(); - tf_broadcaster_.sendTransform(laser_tf_); - } -} - -void Laser::ComputeLaserRanges() { - // get the transformation matrix from the world to the body, and get the - // world to laser frame transformation matrix by multiplying the world to body - // and body to laser - const b2Transform &t = body_->GetPhysicsBody()->GetTransform(); - m_world_to_body_ << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; - m_world_to_laser_ = m_world_to_body_ * m_body_to_laser_; - - // Get the laser points in the world frame by multiplying the laser points in - // the laser frame to the transformation matrix from world to laser frame - m_world_laser_points_ = m_world_to_laser_ * m_laser_points_; - // Get the (0, 0) point in the laser frame - v_world_laser_origin_ = m_world_to_laser_ * v_zero_point_; - - // Conver to Box2D data types - b2Vec2 laser_origin_point(v_world_laser_origin_(0), v_world_laser_origin_(1)); - - // Results vector - std::vector>> results( - laser_scan_.ranges.size()); - - // loop through the laser points and call the Box2D world raycast by - // enqueueing the callback - for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { - results[i] = - pool_.enqueue([i, this, laser_origin_point] { // Lambda function - b2Vec2 laser_point; - laser_point.x = m_world_laser_points_(0, i); - laser_point.y = m_world_laser_points_(1, i); - LaserCallback cb(this); - - GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point, - laser_point); - - if (!cb.did_hit_) { - return std::make_pair(NAN, 0); - } else { - return std::make_pair(cb.fraction_ * this->range_, - cb.intensity_); - } - }); - } - - // Unqueue all of the future'd results - for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { - auto result = results[i].get(); // Pull the result from the future - laser_scan_.ranges[i] = result.first + this->noise_gen_(this->rng_); - if (reflectance_layers_bits_) laser_scan_.intensities[i] = result.second; - } -} - -float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) { - uint16_t category_bits = fixture->GetFilterData().categoryBits; - // only register hit in the specified layers - if (!(category_bits & parent_->layers_bits_)) { - return -1.0f; // return -1 to ignore this hit - } - - // Don't return on hitting sensors... they're not real - if (fixture->IsSensor()) return -1.0f; - - if (category_bits & parent_->reflectance_layers_bits_) { - intensity_ = 255.0; - } - - did_hit_ = true; - fraction_ = fraction; - - return fraction; -} - -void Laser::ParseParameters(const YAML::Node &config) { - YamlReader reader(config); - std::string body_name = reader.Get("body"); - topic_ = reader.Get("topic", "scan"); - frame_id_ = reader.Get("frame", GetName()); - broadcast_tf_ = reader.Get("broadcast_tf", true); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); - origin_ = reader.GetPose("origin", Pose(0, 0, 0)); - range_ = reader.Get("range"); - noise_std_dev_ = reader.Get("noise_std_dev", 0); - - std::vector layers = - reader.GetList("layers", {"all"}, -1, -1); - - YamlReader angle_reader = reader.Subnode("angle", YamlReader::MAP); - min_angle_ = angle_reader.Get("min"); - max_angle_ = angle_reader.Get("max"); - increment_ = angle_reader.Get("increment"); - - angle_reader.EnsureAccessedAllKeys(); - reader.EnsureAccessedAllKeys(); - - if (max_angle_ < min_angle_) { - throw YAMLException("Invalid \"angle\" params, must have max > min"); - } - - body_ = GetModel()->GetBody(body_name); - if (!body_) { - throw YAMLException("Cannot find body with name " + body_name); - } - - std::vector invalid_layers; - layers_bits_ = GetModel()->GetCfr()->GetCategoryBits(layers, &invalid_layers); - if (!invalid_layers.empty()) { - throw YAMLException("Cannot find layer(s): {" + - boost::algorithm::join(invalid_layers, ",") + "}"); - } - - std::vector reflectance_layer = {"reflectance"}; - reflectance_layers_bits_ = - GetModel()->GetCfr()->GetCategoryBits(reflectance_layer, &invalid_layers); - - // init the random number generators - std::random_device rd; - rng_ = std::default_random_engine(rd()); - noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); - - ROS_DEBUG_NAMED("LaserPlugin", - "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " - "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " - "noise_std_dev(%f) angle_min(%f) angle_max(%f) " - "angle_increment(%f) layers(0x%u {%s})", - GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, - origin_.x, origin_.y, origin_.theta, frame_id_.c_str(), - broadcast_tf_, update_rate_, range_, noise_std_dev_, - min_angle_, max_angle_, increment_, layers_bits_, - boost::algorithm::join(layers, ",").c_str()); -} -}; - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin) +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name laser.cpp + * @brief Laser plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +void Laser::OnInitialize(const YAML::Node &config) { + ParseParameters(config); + + update_timer_.SetRate(update_rate_); + scan_publisher_ = nh_.advertise(topic_, 1); + + // construct the body to laser transformation matrix once since it never + // changes + double c = cos(origin_.theta); + double s = sin(origin_.theta); + double x = origin_.x, y = origin_.y; + m_body_to_laser_ << c, -s, x, s, c, y, 0, 0, 1; + + unsigned int num_laser_points = + std::lround((max_angle_ - min_angle_) / increment_) + 1; + + // initialize size for the matrix storing the laser points + m_laser_points_ = Eigen::MatrixXf(3, num_laser_points); + m_world_laser_points_ = Eigen::MatrixXf(3, num_laser_points); + v_zero_point_ << 0, 0, 1; + + // pre-calculate the laser points w.r.t to the laser frame, since this never + // changes + for (unsigned int i = 0; i < num_laser_points; i++) { + float angle = min_angle_ + i * increment_; + + float x = range_ * cos(angle); + float y = range_ * sin(angle); + + m_laser_points_(0, i) = x; + m_laser_points_(1, i) = y; + m_laser_points_(2, i) = 1; + } + + // initialize constants in the laser scan message + laser_scan_.angle_min = min_angle_; + laser_scan_.angle_max = max_angle_; + laser_scan_.angle_increment = increment_; + laser_scan_.time_increment = 0; + laser_scan_.scan_time = 0; + laser_scan_.range_min = 0; + laser_scan_.range_max = range_; + laser_scan_.ranges.resize(num_laser_points); + if (reflectance_layers_bits_) + laser_scan_.intensities.resize(num_laser_points); + else + laser_scan_.intensities.resize(0); + laser_scan_.header.seq = 0; + laser_scan_.header.frame_id = + tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); + + // Broadcast transform between the body and laser + tf::Quaternion q; + q.setRPY(0, 0, origin_.theta); + + laser_tf_.header.frame_id = tf::resolve( + "", GetModel()->NameSpaceTF(body_->GetName())); // Todo: parent_tf param + laser_tf_.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(frame_id_)); + laser_tf_.transform.translation.x = origin_.x; + laser_tf_.transform.translation.y = origin_.y; + laser_tf_.transform.translation.z = 0; + laser_tf_.transform.rotation.x = q.x(); + laser_tf_.transform.rotation.y = q.y(); + laser_tf_.transform.rotation.z = q.z(); + laser_tf_.transform.rotation.w = q.w(); +} + +void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { + // keep the update rate + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + + // only compute and publish when the number of subscribers is not zero + if (scan_publisher_.getNumSubscribers() > 0) { + ComputeLaserRanges(); + laser_scan_.header.stamp = timekeeper.GetSimTime(); + scan_publisher_.publish(laser_scan_); + } + + if (broadcast_tf_) { + laser_tf_.header.stamp = timekeeper.GetSimTime(); + tf_broadcaster_.sendTransform(laser_tf_); + } +} + +void Laser::ComputeLaserRanges() { + // get the transformation matrix from the world to the body, and get the + // world to laser frame transformation matrix by multiplying the world to body + // and body to laser + b2Transform t = b2Body_GetTransform(body_->GetPhysicsBody()); + m_world_to_body_ << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; + m_world_to_laser_ = m_world_to_body_ * m_body_to_laser_; + + // Get the laser points in the world frame + m_world_laser_points_ = m_world_to_laser_ * m_laser_points_; + // Get the (0, 0) point in the laser frame + v_world_laser_origin_ = m_world_to_laser_ * v_zero_point_; + + // Convert to Box2D data types + b2Vec2 laser_origin_point = {v_world_laser_origin_(0), + v_world_laser_origin_(1)}; + + b2WorldId world_id = GetModel()->GetPhysicsWorld(); + + // Sequential ray cast loop (thread pool replaced; physics is now MT via enkiTS) + for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { + b2Vec2 laser_point = {m_world_laser_points_(0, i), + m_world_laser_points_(1, i)}; + + LaserRayContext ctx; + ctx.layers_bits = layers_bits_; + ctx.reflectance_layers_bits = reflectance_layers_bits_; + + b2QueryFilter filter = b2DefaultQueryFilter(); + filter.maskBits = layers_bits_; + b2World_CastRay(world_id, laser_origin_point, laser_point, filter, + LaserRayCastFcn, &ctx); + + if (!ctx.did_hit) { + laser_scan_.ranges[i] = NAN; + } else { + laser_scan_.ranges[i] = + static_cast(ctx.fraction * range_ + noise_gen_(rng_)); + } + if (reflectance_layers_bits_) laser_scan_.intensities[i] = ctx.intensity; + } +} + +float LaserRayCastFcn(b2ShapeId shapeId, b2Vec2 /*point*/, b2Vec2 /*normal*/, + float fraction, void *context) { + auto *ctx = static_cast(context); + b2Filter filter = b2Shape_GetFilter(shapeId); + uint16_t category_bits = static_cast(filter.categoryBits); + + // only register hit in the specified layers + if (!(category_bits & ctx->layers_bits)) { + return -1.0f; // ignore this hit + } + + // Don't hit sensors + if (b2Shape_IsSensor(shapeId)) return -1.0f; + + if (category_bits & ctx->reflectance_layers_bits) { + ctx->intensity = 255.0f; + } + + ctx->did_hit = true; + ctx->fraction = fraction; + + return fraction; // return fraction to find closest hit +} + +void Laser::ParseParameters(const YAML::Node &config) { + YamlReader reader(config); + std::string body_name = reader.Get("body"); + topic_ = reader.Get("topic", "scan"); + frame_id_ = reader.Get("frame", GetName()); + broadcast_tf_ = reader.Get("broadcast_tf", true); + update_rate_ = reader.Get("update_rate", + std::numeric_limits::infinity()); + origin_ = reader.GetPose("origin", Pose(0, 0, 0)); + range_ = reader.Get("range"); + noise_std_dev_ = reader.Get("noise_std_dev", 0); + + std::vector layers = + reader.GetList("layers", {"all"}, -1, -1); + + YamlReader angle_reader = reader.Subnode("angle", YamlReader::MAP); + min_angle_ = angle_reader.Get("min"); + max_angle_ = angle_reader.Get("max"); + increment_ = angle_reader.Get("increment"); + + angle_reader.EnsureAccessedAllKeys(); + reader.EnsureAccessedAllKeys(); + + if (max_angle_ < min_angle_) { + throw YAMLException("Invalid \"angle\" params, must have max > min"); + } + + body_ = GetModel()->GetBody(body_name); + if (!body_) { + throw YAMLException("Cannot find body with name " + body_name); + } + + std::vector invalid_layers; + layers_bits_ = GetModel()->GetCfr()->GetCategoryBits(layers, &invalid_layers); + if (!invalid_layers.empty()) { + throw YAMLException("Cannot find layer(s): {" + + boost::algorithm::join(invalid_layers, ",") + "}"); + } + + std::vector reflectance_layer = {"reflectance"}; + reflectance_layers_bits_ = + GetModel()->GetCfr()->GetCategoryBits(reflectance_layer, &invalid_layers); + + // init the random number generators + std::random_device rd; + rng_ = std::default_random_engine(rd()); + noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); + + ROS_DEBUG_NAMED("LaserPlugin", + "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " + "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " + "noise_std_dev(%f) angle_min(%f) angle_max(%f) " + "angle_increment(%f) layers(0x%u {%s})", + GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, + origin_.x, origin_.y, origin_.theta, frame_id_.c_str(), + broadcast_tf_, update_rate_, range_, noise_std_dev_, + min_angle_, max_angle_, increment_, layers_bits_, + boost::algorithm::join(layers, ",").c_str()); +} +}; + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/model_tf_publisher.cpp b/flatland_plugins/src/model_tf_publisher.cpp index 4e0fdb0c..119f0276 100644 --- a/flatland_plugins/src/model_tf_publisher.cpp +++ b/flatland_plugins/src/model_tf_publisher.cpp @@ -1,196 +1,196 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_tf_publisher.cpp - * @brief Publish tf in robots - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -void ModelTfPublisher::OnInitialize(const YAML::Node &config) { - YamlReader reader(config); - - // default values - publish_tf_world_ = reader.Get("publish_tf_world", false); - world_frame_id_ = reader.Get("world_frame_id", "map"); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); - - std::string ref_body_name = reader.Get("reference", ""); - std::vector excluded_body_names = - reader.GetList("exclude", {}, -1, -1); - reader.EnsureAccessedAllKeys(); - - if (ref_body_name.size() != 0) { - reference_body_ = GetModel()->GetBody(ref_body_name); - - if (reference_body_ == nullptr) { - throw YAMLException("Body with name \"" + ref_body_name + - "\" does not exist"); - } - } else { - // defaults to the first body, the reference body has no effect on the - // final result, but it changes how the TF would look - reference_body_ = GetModel()->bodies_[0]; - } - - for (unsigned int i = 0; i < excluded_body_names.size(); i++) { - Body *body = GetModel()->GetBody(excluded_body_names[i]); - - if (body == nullptr) { - throw YAMLException("Body with name \"" + excluded_body_names[i] + - "\" does not exist"); - } else { - excluded_bodies_.push_back(body); - } - } - - update_timer_.SetRate(update_rate_); - - ROS_DEBUG_NAMED( - "ModelTfPublisher", - "Initialized with params: reference(%s, %p) " - "publish_tf_world(%d) world_frame_id(%s) update_rate(%f), exclude({%s})", - reference_body_->name_.c_str(), reference_body_, publish_tf_world_, - world_frame_id_.c_str(), update_rate_, - boost::algorithm::join(excluded_body_names, ",").c_str()); -} - -void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - - Eigen::Matrix3f ref_tf_m; ///< for storing TF from world to the ref. body - Eigen::Matrix3f rel_tf; ///< for storing TF from ref. body to other bodies - - // fill the world to ref. body TF with data from Box2D - const b2Transform &r = reference_body_->physics_body_->GetTransform(); - ref_tf_m << r.q.c, -r.q.s, r.p.x, r.q.s, r.q.c, r.p.y, 0, 0, 1; - - geometry_msgs::TransformStamped tf_stamped; - tf_stamped.header.stamp = timekeeper.GetSimTime(); - - // loop through the bodies to calculate TF, and ignores excluded bodies - for (unsigned int i = 0; i < GetModel()->bodies_.size(); i++) { - Body *body = GetModel()->bodies_[i]; - bool is_excluded = false; - - for (unsigned int j = 0; j < excluded_bodies_.size(); j++) { - if (body == excluded_bodies_[j]) { - is_excluded = true; - } - } - - if (is_excluded || body == reference_body_) { - continue; - } - - // Get transformation of body w.r.t to the world - const b2Transform &b = body->physics_body_->GetTransform(); - Eigen::Matrix3f body_tf_m; - body_tf_m << b.q.c, -b.q.s, b.p.x, b.q.s, b.q.c, b.p.y, 0, 0, 1; - - // this calculates the transformation from the reference body to the - // other body. It is needed because Box2D only provides position and - // angle of bodies w.r.t to the world - rel_tf = ref_tf_m.inverse() * body_tf_m; - - // obtain the yaw from the transformation matrice - double cosine = rel_tf(0, 0); - double sine = rel_tf(1, 0); - double yaw = atan2(sine, cosine); - - // publish TF - tf_stamped.header.frame_id = - tf::resolve("", GetModel()->NameSpaceTF(reference_body_->name_)); - tf_stamped.child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(body->name_)); - tf_stamped.transform.translation.x = rel_tf(0, 2); - tf_stamped.transform.translation.y = rel_tf(1, 2); - tf_stamped.transform.translation.z = 0; - tf::Quaternion q; - q.setRPY(0, 0, yaw); - tf_stamped.transform.rotation.x = q.x(); - tf_stamped.transform.rotation.y = q.y(); - tf_stamped.transform.rotation.z = q.z(); - tf_stamped.transform.rotation.w = q.w(); - - tf_broadcaster.sendTransform(tf_stamped); - } - - // publish world TF if necessary - if (publish_tf_world_) { - const b2Vec2 &p = reference_body_->physics_body_->GetPosition(); - double yaw = reference_body_->physics_body_->GetAngle(); - - tf_stamped.header.frame_id = world_frame_id_; - tf_stamped.child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(reference_body_->name_)); - tf_stamped.transform.translation.x = p.x; - tf_stamped.transform.translation.y = p.y; - tf_stamped.transform.translation.z = 0; - tf::Quaternion q; - q.setRPY(0, 0, yaw); - tf_stamped.transform.rotation.x = q.x(); - tf_stamped.transform.rotation.y = q.y(); - tf_stamped.transform.rotation.z = q.z(); - tf_stamped.transform.rotation.w = q.w(); - - tf_broadcaster.sendTransform(tf_stamped); - } -} -}; - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::ModelTfPublisher, - flatland_server::ModelPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_tf_publisher.cpp + * @brief Publish tf in robots + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +void ModelTfPublisher::OnInitialize(const YAML::Node &config) { + YamlReader reader(config); + + // default values + publish_tf_world_ = reader.Get("publish_tf_world", false); + world_frame_id_ = reader.Get("world_frame_id", "map"); + update_rate_ = reader.Get("update_rate", + std::numeric_limits::infinity()); + + std::string ref_body_name = reader.Get("reference", ""); + std::vector excluded_body_names = + reader.GetList("exclude", {}, -1, -1); + reader.EnsureAccessedAllKeys(); + + if (ref_body_name.size() != 0) { + reference_body_ = GetModel()->GetBody(ref_body_name); + + if (reference_body_ == nullptr) { + throw YAMLException("Body with name \"" + ref_body_name + + "\" does not exist"); + } + } else { + // defaults to the first body, the reference body has no effect on the + // final result, but it changes how the TF would look + reference_body_ = GetModel()->bodies_[0]; + } + + for (unsigned int i = 0; i < excluded_body_names.size(); i++) { + Body *body = GetModel()->GetBody(excluded_body_names[i]); + + if (body == nullptr) { + throw YAMLException("Body with name \"" + excluded_body_names[i] + + "\" does not exist"); + } else { + excluded_bodies_.push_back(body); + } + } + + update_timer_.SetRate(update_rate_); + + ROS_DEBUG_NAMED( + "ModelTfPublisher", + "Initialized with params: reference(%s, %p) " + "publish_tf_world(%d) world_frame_id(%s) update_rate(%f), exclude({%s})", + reference_body_->name_.c_str(), reference_body_, publish_tf_world_, + world_frame_id_.c_str(), update_rate_, + boost::algorithm::join(excluded_body_names, ",").c_str()); +} + +void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + + Eigen::Matrix3f ref_tf_m; ///< for storing TF from world to the ref. body + Eigen::Matrix3f rel_tf; ///< for storing TF from ref. body to other bodies + + // fill the world to ref. body TF with data from Box2D + b2Transform r = b2Body_GetTransform(reference_body_->physics_body_); + ref_tf_m << r.q.c, -r.q.s, r.p.x, r.q.s, r.q.c, r.p.y, 0, 0, 1; + + geometry_msgs::TransformStamped tf_stamped; + tf_stamped.header.stamp = timekeeper.GetSimTime(); + + // loop through the bodies to calculate TF, and ignores excluded bodies + for (unsigned int i = 0; i < GetModel()->bodies_.size(); i++) { + Body *body = GetModel()->bodies_[i]; + bool is_excluded = false; + + for (unsigned int j = 0; j < excluded_bodies_.size(); j++) { + if (body == excluded_bodies_[j]) { + is_excluded = true; + } + } + + if (is_excluded || body == reference_body_) { + continue; + } + + // Get transformation of body w.r.t to the world + b2Transform b = b2Body_GetTransform(body->physics_body_); + Eigen::Matrix3f body_tf_m; + body_tf_m << b.q.c, -b.q.s, b.p.x, b.q.s, b.q.c, b.p.y, 0, 0, 1; + + // this calculates the transformation from the reference body to the + // other body. It is needed because Box2D only provides position and + // angle of bodies w.r.t to the world + rel_tf = ref_tf_m.inverse() * body_tf_m; + + // obtain the yaw from the transformation matrice + double cosine = rel_tf(0, 0); + double sine = rel_tf(1, 0); + double yaw = atan2(sine, cosine); + + // publish TF + tf_stamped.header.frame_id = + tf::resolve("", GetModel()->NameSpaceTF(reference_body_->name_)); + tf_stamped.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(body->name_)); + tf_stamped.transform.translation.x = rel_tf(0, 2); + tf_stamped.transform.translation.y = rel_tf(1, 2); + tf_stamped.transform.translation.z = 0; + tf::Quaternion q; + q.setRPY(0, 0, yaw); + tf_stamped.transform.rotation.x = q.x(); + tf_stamped.transform.rotation.y = q.y(); + tf_stamped.transform.rotation.z = q.z(); + tf_stamped.transform.rotation.w = q.w(); + + tf_broadcaster.sendTransform(tf_stamped); + } + + // publish world TF if necessary + if (publish_tf_world_) { + b2Vec2 p = b2Body_GetPosition(reference_body_->physics_body_); + double yaw = b2Rot_GetAngle(b2Body_GetRotation(reference_body_->physics_body_)); + + tf_stamped.header.frame_id = world_frame_id_; + tf_stamped.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(reference_body_->name_)); + tf_stamped.transform.translation.x = p.x; + tf_stamped.transform.translation.y = p.y; + tf_stamped.transform.translation.z = 0; + tf::Quaternion q; + q.setRPY(0, 0, yaw); + tf_stamped.transform.rotation.x = q.x(); + tf_stamped.transform.rotation.y = q.y(); + tf_stamped.transform.rotation.z = q.z(); + tf_stamped.transform.rotation.w = q.w(); + + tf_broadcaster.sendTransform(tf_stamped); + } +} +}; + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::ModelTfPublisher, + flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/tricycle_drive.cpp b/flatland_plugins/src/tricycle_drive.cpp index 426325fc..25a45177 100644 --- a/flatland_plugins/src/tricycle_drive.cpp +++ b/flatland_plugins/src/tricycle_drive.cpp @@ -1,447 +1,470 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name tricycle_drive.cpp - * @brief tricycle plugin - * @author Mike Brousseau, Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_plugins { - -void TricycleDrive::OnInitialize(const YAML::Node& config) { - YamlReader r(config); - - // load all the parameters - string body_name = r.Get("body"); - string front_wj_name = r.Get("front_wheel_joint"); - string rear_left_wj_name = r.Get("rear_left_wheel_joint"); - string rear_right_wj_name = r.Get("rear_right_wheel_joint"); - string odom_frame_id = r.Get("odom_frame_id", "odom"); - - string twist_topic = r.Get("twist_sub", "cmd_vel"); - string odom_topic = r.Get("odom_pub", "odometry/filtered"); - string ground_truth_topic = - r.Get("ground_truth_pub", "odometry/ground_truth"); - - // noise are in the form of linear x, linear y, angular variances - vector odom_twist_noise = - r.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); - vector odom_pose_noise = - r.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); - - double pub_rate = - r.Get("pub_rate", numeric_limits::infinity()); - update_timer_.SetRate(pub_rate); - - // by default the covariance diagonal is the variance of actual noise - // generated, non-diagonal elements are zero assuming the noises are - // independent, we also don't care about linear z, angular x, and angular y - array odom_pose_covar_default = {0}; - odom_pose_covar_default[0] = odom_pose_noise[0]; - odom_pose_covar_default[7] = odom_pose_noise[1]; - odom_pose_covar_default[35] = odom_pose_noise[2]; - - array odom_twist_covar_default = {0}; - odom_twist_covar_default[0] = odom_twist_noise[0]; - odom_twist_covar_default[7] = odom_twist_noise[1]; - odom_twist_covar_default[35] = odom_twist_noise[2]; - - auto odom_twist_covar = - r.GetArray("odom_twist_covariance", odom_twist_covar_default); - auto odom_pose_covar = - r.GetArray("odom_pose_covariance", odom_pose_covar_default); - - // Default max_steer_angle=0.0 means "unbounded" - max_steer_angle_ = r.Get("max_steer_angle", 0.0); - - // Angular dynamics constraints - angular_dynamics_.Configure(r.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node()); - - // Accept old configuration location for angular dynamics constraints if present - if (angular_dynamics_.velocity_limit_ != 0.0) angular_dynamics_.velocity_limit_ = r.Get("max_angular_velocity", 0.0); - if (angular_dynamics_.acceleration_limit_ != 0.0) { - angular_dynamics_.acceleration_limit_ = r.Get("max_steer_acceleration", 0.0); - angular_dynamics_.deceleration_limit_ = angular_dynamics_.acceleration_limit_ ; - } - - // Linear dynamics constraints - linear_dynamics_.Configure(r.SubnodeOpt("linear_dynamics", YamlReader::MAP).Node()); - - delta_command_ = 0.0; - theta_f_ = 0.0; - d_delta_ = 0.0; - - r.EnsureAccessedAllKeys(); - - // Get the bodies and joints from names, throw if not found - body_ = GetModel()->GetBody(body_name); - if (body_ == nullptr) { - throw YAMLException("Body with name " + Q(body_name) + " does not exist"); - } - - front_wj_ = GetModel()->GetJoint(front_wj_name); - if (front_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(front_wj_name) + - " does not exist"); - } - - rear_left_wj_ = GetModel()->GetJoint(rear_left_wj_name); - if (rear_left_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(rear_left_wj_name) + - " does not exist"); - } - - rear_right_wj_ = GetModel()->GetJoint(rear_right_wj_name); - if (rear_right_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(rear_right_wj_name) + - " does not exist"); - } - - // validate the that joints fits the assumption of the robot model and - // calculate rear wheel separation and wheel base - ComputeJoints(); - - // publish and subscribe to topics - twist_sub_ = - nh_.subscribe(twist_topic, 1, &TricycleDrive::TwistCallback, this); - odom_pub_ = nh_.advertise(odom_topic, 1); - ground_truth_pub_ = nh_.advertise(ground_truth_topic, 1); - - // init the values for the messages - ground_truth_msg_.header.frame_id = odom_frame_id; - ground_truth_msg_.child_frame_id = - tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); - - ground_truth_msg_.twist.covariance.fill(0); - ground_truth_msg_.pose.covariance.fill(0); - odom_msg_ = ground_truth_msg_; - - // copy from array to boost array - for (unsigned int i = 0; i < 36; i++) { - odom_msg_.twist.covariance[i] = odom_twist_covar[i]; - odom_msg_.pose.covariance[i] = odom_pose_covar[i]; - } - - // init the random number generators - random_device rd; - rng_ = default_random_engine(rd()); - for (unsigned int i = 0; i < 3; i++) { - // variance is standard deviation squared - noise_gen_[i] = normal_distribution(0.0, sqrt(odom_pose_noise[i])); - } - - for (unsigned int i = 0; i < 3; i++) { - noise_gen_[i + 3] = - normal_distribution(0.0, sqrt(odom_twist_noise[i])); - } - - ROS_DEBUG_NAMED( - "TricycleDrive", - "Initialized with params body(%p %s) front_wj(%p %s) " - "rear_left_wj(%p %s) rear_right_wj(%p %s) " - "odom_frame_id(%s) twist_sub(%s) odom_pub(%s) " - "ground_truth_pub(%s) odom_pose_noise({%f,%f,%f}) " - "odom_twist_noise({%f,%f,%f}) pub_rate(%f)\n", - body_, body_->GetName().c_str(), front_wj_, front_wj_->GetName().c_str(), - rear_left_wj_, rear_left_wj_->GetName().c_str(), rear_right_wj_, - rear_right_wj_->GetName().c_str(), odom_frame_id.c_str(), - twist_topic.c_str(), odom_topic.c_str(), ground_truth_topic.c_str(), - odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], - odom_twist_noise[0], odom_twist_noise[1], odom_twist_noise[2], pub_rate); -} - -void TricycleDrive::ComputeJoints() { - auto get_anchor = [&](Joint* joint, bool* is_inverted = nullptr) { - - b2Vec2 wheel_anchor; ///< wheel anchor point, must be (0,0) - b2Vec2 body_anchor; ///< body anchor point - bool inv = false; - - // ensure one of the body is the main body for the odometry - if (joint->physics_joint_->GetBodyA()->GetUserData() == body_) { - wheel_anchor = joint->physics_joint_->GetAnchorB(); - body_anchor = joint->physics_joint_->GetAnchorA(); - } else if (joint->physics_joint_->GetBodyB()->GetUserData() == body_) { - wheel_anchor = joint->physics_joint_->GetAnchorA(); - body_anchor = joint->physics_joint_->GetAnchorB(); - inv = true; - } else { - throw YAMLException("Joint " + Q(joint->GetName()) + - " does not anchor on body " + Q(body_->GetName())); - } - - // convert anchor is global coordinates to local body coordinates - wheel_anchor = body_->physics_body_->GetLocalPoint(wheel_anchor); - body_anchor = body_->physics_body_->GetLocalPoint(body_anchor); - - // ensure the joint is anchored at (0,0) of the wheel_body - if (fabs(wheel_anchor.x) > 1e-5 || fabs(wheel_anchor.y) > 1e-5) { - throw YAMLException("Joint " + Q(joint->GetName()) + - " must be anchored at (0, 0) on the wheel"); - } - - if (is_inverted) { - *is_inverted = inv; - } - - return body_anchor; - }; - - // joints must be of expected type - if (front_wj_->physics_joint_->GetType() != e_revoluteJoint) { - throw YAMLException("Front wheel joint must be a revolute joint"); - } - - if (rear_left_wj_->physics_joint_->GetType() != e_weldJoint) { - throw YAMLException("Rear left wheel joint must be a weld joint"); - } - - if (rear_right_wj_->physics_joint_->GetType() != e_weldJoint) { - throw YAMLException("Rear right wheel joint must be a weld joint"); - } - - // enable limits for the front joint - b2RevoluteJoint* j = - dynamic_cast(front_wj_->physics_joint_); - j->EnableLimit(true); - - // positive joint angle goes counter clockwise from the perspective of BodyA, - // if body_ is not BodyA, we need flip the steering angle for visualization - b2Vec2 front_anchor = get_anchor(front_wj_, &invert_steering_angle_); - b2Vec2 rear_left_anchor = get_anchor(rear_left_wj_); - b2Vec2 rear_right_anchor = get_anchor(rear_right_wj_); - - // the front wheel must be at (0,0) of the body - if (fabs(front_anchor.x) > 1e-5 || fabs(front_anchor.y) > 1e-5) { - throw YAMLException( - "Front wheel joint must have its body anchored at (0, 0)"); - } - - // calculate the wheelbase and axeltrack. We also need to verify that - // the rear_center is at the perpendicular intersection between the rear axel - // and the front wheel anchor - rear_center_ = 0.5 * (rear_left_anchor + rear_right_anchor); - - // find the perpendicular intersection between line segment given by (x1, y1) - // and (x2, y2) and a point (x3, y3). - double x1 = rear_left_anchor.x, y1 = rear_left_anchor.y, - x2 = rear_right_anchor.x, y2 = rear_right_anchor.y, - x3 = front_anchor.x, y3 = front_anchor.y; - - double k = ((y2 - y1) * (x3 - x1) - (x2 - x1) * (y3 - y1)) / - ((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1)); - double x4 = x3 - k * (y2 - y1); - double y4 = y3 + k * (x2 - x1); - - // check (x4, y4) equals to rear_center_ - if (fabs(x4 - rear_center_.x) > 1e-5 || fabs(y4 - rear_center_.y) > 1e-5) { - throw YAMLException( - "The mid point between the rear wheel anchors on the body must equal " - "the perpendicular intersection between the rear axel (line segment " - "between rear anchors) and the front wheel anchor"); - } - - // track is the separation between the rear two wheels, which is simply the - // distance between the rear two wheels - axel_track_ = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); - - // wheel base is the perpendicular distance between the rear axel and the - // front wheel - wheelbase_ = sqrt((x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3)); -} - -void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { - bool publish = update_timer_.CheckUpdate(timekeeper); - - b2Body* b2body = body_->physics_body_; - - b2Vec2 position = b2body->GetPosition(); - float angle = b2body->GetAngle(); - - if (publish) { - // 1. get the state of the body and publish the data, - // before the tricycle physics get updated - b2Vec2 linear_vel_local = - b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); - float angular_vel = b2body->GetAngularVelocity(); - - ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); - ground_truth_msg_.pose.pose.position.x = position.x; - ground_truth_msg_.pose.pose.position.y = position.y; - ground_truth_msg_.pose.pose.position.z = 0; - ground_truth_msg_.pose.pose.orientation = - tf::createQuaternionMsgFromYaw(angle); - ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x; - ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y; - ground_truth_msg_.twist.twist.linear.z = 0; - ground_truth_msg_.twist.twist.angular.x = 0; - ground_truth_msg_.twist.twist.angular.y = 0; - ground_truth_msg_.twist.twist.angular.z = angular_vel; - - // add the noise to odom messages - odom_msg_.header.stamp = timekeeper.GetSimTime(); - odom_msg_.pose.pose = ground_truth_msg_.pose.pose; - odom_msg_.twist.twist = ground_truth_msg_.twist.twist; - odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); - odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); - odom_msg_.pose.pose.orientation = - tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); - odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); - odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); - odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); - - ground_truth_pub_.publish(ground_truth_msg_); - odom_pub_.publish(odom_msg_); - } - - // 2. Update the tricycle physics based on the twist command - // This is a highly simplified kinematic approximation of the response - // of the steering and drive mechanisms. - - // Equations of motion for steering (kinematics only) - // Let δ (delta) = measured steering angle - // δ_c = commanded steering angle - // Then the kinematic discrete-time equations are (1st-order approximation): - // (1) δ[t+1] = δ[t] + dδ[t+1] * dt new steering angle - // (2) dδ[t+1] = dδ[t] + d2δ[t+1] * dt new steering velocity - // (3) d2δ[t+1] = (dδ_c[t+1] - dδ[t]) / dt new steering acceleration - // (4) dδ_c[t+1] = (δ_c[t+1] - δ[t]) / dt new steering velocity command - // when it requires > 1 dt to reach δ[t+1] - // 0.0 - // otherwise - // subject to: - // |δ[t]| <= max_steer_angle_ - // |dδ[t]| <= max_steer_velocity_ - // |d2δ[t]| <= max_steer_acceleration_ - - // twist message contains the speed and angle of the front wheel - delta_command_ = twist_msg_.angular.z; // target steering angle - double theta = angle; // angle of robot in map frame - double dt = timekeeper.GetStepSize(); - - // In the simulation, the equations of motion have to be computed backwards - // (4) Update the new commanded steering velocity - // Note: Set target steer velocity = 0 rad/s to avoid overshooting, when - // it is possible to reach the commanded steering angle in 1 step - double d_delta_command = 0.0; - double delta_max_one_step = d_delta_ * d_delta_ / 2 / max_steer_acceleration_; - if (max_steer_acceleration_ == 0.0) { - delta_max_one_step = fabs(delta_command_ - theta_f_); - } - - if (fabs(delta_command_ - theta_f_) >= delta_max_one_step) { - d_delta_command = (delta_command_ - theta_f_) / dt; - } - - // Apply angular dynamics constraints - d_delta_ = angular_dynamics_.Limit(d_delta_, d_delta_command, dt); - - // (1) Update the new steering angle - theta_f_ += d_delta_ * dt; - if (max_steer_angle_ != 0.0) { - theta_f_ = DynamicsLimits::Saturate(theta_f_, -max_steer_angle_, max_steer_angle_); - } - - ROS_DEBUG_THROTTLE(0.5, - "Using new tricycle steering, " - "d_delta = %.4f, twist.x = %.4f, twist.delta = %.4f", - d_delta_, twist_msg_.linear.x, - twist_msg_.angular.z); - - // change angle of the front wheel for visualization - - b2RevoluteJoint* j = - dynamic_cast(front_wj_->physics_joint_); - j->EnableLimit(true); - if (invert_steering_angle_) { - j->SetLimits(-theta_f_, -theta_f_); - } else { - j->SetLimits(theta_f_, theta_f_); - } - - // calculate the desired velocity using the bicycle model in the world frame - // looking at the rear center, formulas obtained from avidbots robot systems - // confluence page - - // apply linear velocity and acceleration constraints - v_f_ = linear_dynamics_.Limit(v_f_, twist_msg_.linear.x, dt); - - double v_x = v_f_ * cos(theta_f_) * cos(theta); // x velocity in world - double v_y = v_f_ * cos(theta_f_) * sin(theta); // y velocity in world - double w = v_f_ * sin(theta_f_) / wheelbase_; // angular velocity - - // Now we would like the rear center to move at v_x, v_y, and w, since Box2D - // applies velocities at center of mass, we must use rigid body kinematics - // to transform the velocities - b2Vec2 linear_vel(v_x, v_y); - - // V_cm = V_rc + W x r_cm/rc - // velocity at center of mass equals to the velocity at the rear center plus, - // angular velocity cross product the displacement from the rear center to the - // center of mass - - // r is the vector from rear center to CM in world frame - b2Vec2 r = b2body->GetWorldCenter() - b2body->GetWorldPoint(rear_center_); - b2Vec2 linear_vel_cm = linear_vel + w * b2Vec2(-r.y, r.x); - - b2body->SetLinearVelocity(linear_vel_cm); - - // angular velocity is the same at any point in body - b2body->SetAngularVelocity(w); -} - -void TricycleDrive::TwistCallback(const geometry_msgs::Twist& msg) { - twist_msg_ = msg; -} - -} - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::TricycleDrive, - flatland_server::ModelPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name tricycle_drive.cpp + * @brief tricycle plugin + * @author Mike Brousseau, Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_plugins { + +void TricycleDrive::OnInitialize(const YAML::Node& config) { + YamlReader r(config); + + // load all the parameters + string body_name = r.Get("body"); + string front_wj_name = r.Get("front_wheel_joint"); + string rear_left_wj_name = r.Get("rear_left_wheel_joint"); + string rear_right_wj_name = r.Get("rear_right_wheel_joint"); + string odom_frame_id = r.Get("odom_frame_id", "odom"); + + string twist_topic = r.Get("twist_sub", "cmd_vel"); + string odom_topic = r.Get("odom_pub", "odometry/filtered"); + string ground_truth_topic = + r.Get("ground_truth_pub", "odometry/ground_truth"); + + // noise are in the form of linear x, linear y, angular variances + vector odom_twist_noise = + r.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); + vector odom_pose_noise = + r.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); + + double pub_rate = + r.Get("pub_rate", numeric_limits::infinity()); + update_timer_.SetRate(pub_rate); + + // by default the covariance diagonal is the variance of actual noise + // generated, non-diagonal elements are zero assuming the noises are + // independent, we also don't care about linear z, angular x, and angular y + array odom_pose_covar_default = {0}; + odom_pose_covar_default[0] = odom_pose_noise[0]; + odom_pose_covar_default[7] = odom_pose_noise[1]; + odom_pose_covar_default[35] = odom_pose_noise[2]; + + array odom_twist_covar_default = {0}; + odom_twist_covar_default[0] = odom_twist_noise[0]; + odom_twist_covar_default[7] = odom_twist_noise[1]; + odom_twist_covar_default[35] = odom_twist_noise[2]; + + auto odom_twist_covar = + r.GetArray("odom_twist_covariance", odom_twist_covar_default); + auto odom_pose_covar = + r.GetArray("odom_pose_covariance", odom_pose_covar_default); + + // Default max_steer_angle=0.0 means "unbounded" + max_steer_angle_ = r.Get("max_steer_angle", 0.0); + + // Angular dynamics constraints + angular_dynamics_.Configure(r.SubnodeOpt("angular_dynamics", YamlReader::MAP).Node()); + + // Accept old configuration location for angular dynamics constraints if present + if (angular_dynamics_.velocity_limit_ != 0.0) angular_dynamics_.velocity_limit_ = r.Get("max_angular_velocity", 0.0); + if (angular_dynamics_.acceleration_limit_ != 0.0) { + angular_dynamics_.acceleration_limit_ = r.Get("max_steer_acceleration", 0.0); + angular_dynamics_.deceleration_limit_ = angular_dynamics_.acceleration_limit_ ; + } + + // Linear dynamics constraints + linear_dynamics_.Configure(r.SubnodeOpt("linear_dynamics", YamlReader::MAP).Node()); + + delta_command_ = 0.0; + theta_f_ = 0.0; + d_delta_ = 0.0; + + r.EnsureAccessedAllKeys(); + + // Get the bodies and joints from names, throw if not found + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name " + Q(body_name) + " does not exist"); + } + + front_wj_ = GetModel()->GetJoint(front_wj_name); + if (front_wj_ == nullptr) { + throw YAMLException("Joint with name " + Q(front_wj_name) + + " does not exist"); + } + + rear_left_wj_ = GetModel()->GetJoint(rear_left_wj_name); + if (rear_left_wj_ == nullptr) { + throw YAMLException("Joint with name " + Q(rear_left_wj_name) + + " does not exist"); + } + + rear_right_wj_ = GetModel()->GetJoint(rear_right_wj_name); + if (rear_right_wj_ == nullptr) { + throw YAMLException("Joint with name " + Q(rear_right_wj_name) + + " does not exist"); + } + + // validate the that joints fits the assumption of the robot model and + // calculate rear wheel separation and wheel base + ComputeJoints(); + + // publish and subscribe to topics + twist_sub_ = + nh_.subscribe(twist_topic, 1, &TricycleDrive::TwistCallback, this); + odom_pub_ = nh_.advertise(odom_topic, 1); + ground_truth_pub_ = nh_.advertise(ground_truth_topic, 1); + + // init the values for the messages + ground_truth_msg_.header.frame_id = odom_frame_id; + ground_truth_msg_.child_frame_id = + tf::resolve("", GetModel()->NameSpaceTF(body_->name_)); + + ground_truth_msg_.twist.covariance.fill(0); + ground_truth_msg_.pose.covariance.fill(0); + odom_msg_ = ground_truth_msg_; + + // copy from array to boost array + for (unsigned int i = 0; i < 36; i++) { + odom_msg_.twist.covariance[i] = odom_twist_covar[i]; + odom_msg_.pose.covariance[i] = odom_pose_covar[i]; + } + + // init the random number generators + random_device rd; + rng_ = default_random_engine(rd()); + for (unsigned int i = 0; i < 3; i++) { + // variance is standard deviation squared + noise_gen_[i] = normal_distribution(0.0, sqrt(odom_pose_noise[i])); + } + + for (unsigned int i = 0; i < 3; i++) { + noise_gen_[i + 3] = + normal_distribution(0.0, sqrt(odom_twist_noise[i])); + } + + ROS_DEBUG_NAMED( + "TricycleDrive", + "Initialized with params body(%p %s) front_wj(%p %s) " + "rear_left_wj(%p %s) rear_right_wj(%p %s) " + "odom_frame_id(%s) twist_sub(%s) odom_pub(%s) " + "ground_truth_pub(%s) odom_pose_noise({%f,%f,%f}) " + "odom_twist_noise({%f,%f,%f}) pub_rate(%f)\n", + body_, body_->GetName().c_str(), front_wj_, front_wj_->GetName().c_str(), + rear_left_wj_, rear_left_wj_->GetName().c_str(), rear_right_wj_, + rear_right_wj_->GetName().c_str(), odom_frame_id.c_str(), + twist_topic.c_str(), odom_topic.c_str(), ground_truth_topic.c_str(), + odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], + odom_twist_noise[0], odom_twist_noise[1], odom_twist_noise[2], pub_rate); +} + +void TricycleDrive::ComputeJoints() { + auto get_anchor = [&](Joint* joint, bool* is_inverted = nullptr) { + + b2Vec2 wheel_anchor; ///< wheel anchor point, must be (0,0) + b2Vec2 body_anchor; ///< body anchor point + bool inv = false; + + b2JointId jid = joint->physics_joint_; + b2BodyId jbodyA = b2Joint_GetBodyA(jid); + b2BodyId jbodyB = b2Joint_GetBodyB(jid); + + // Get world anchor points from a joint (revolute or weld) + b2Vec2 anchorA, anchorB; + { + b2Transform tfA = b2Body_GetTransform(jbodyA); + b2Transform tfB = b2Body_GetTransform(jbodyB); + if (b2Joint_GetType(jid) == b2_revoluteJoint) { + anchorA = b2TransformPoint(tfA, b2Joint_GetLocalAnchorA(jid)); + anchorB = b2TransformPoint(tfB, b2Joint_GetLocalAnchorB(jid)); + } else { + anchorA = b2TransformPoint(tfA, b2Joint_GetLocalAnchorA(jid)); + anchorB = b2TransformPoint(tfB, b2Joint_GetLocalAnchorB(jid)); + } + } + + // ensure one of the body is the main body for the odometry + if (b2Body_GetUserData(jbodyA) == body_) { + wheel_anchor = anchorB; + body_anchor = anchorA; + } else if (b2Body_GetUserData(jbodyB) == body_) { + wheel_anchor = anchorA; + body_anchor = anchorB; + inv = true; + } else { + throw YAMLException("Joint " + Q(joint->GetName()) + + " does not anchor on body " + Q(body_->GetName())); + } + + // convert anchor in global coordinates to local body coordinates + b2Transform bodyTf = b2Body_GetTransform(body_->physics_body_); + wheel_anchor = b2InvTransformPoint(bodyTf, wheel_anchor); + body_anchor = b2InvTransformPoint(bodyTf, body_anchor); + + // ensure the joint is anchored at (0,0) of the wheel_body + if (fabs(wheel_anchor.x) > 1e-5 || fabs(wheel_anchor.y) > 1e-5) { + throw YAMLException("Joint " + Q(joint->GetName()) + + " must be anchored at (0, 0) on the wheel"); + } + + if (is_inverted) { + *is_inverted = inv; + } + + return body_anchor; + }; + + // joints must be of expected type + if (b2Joint_GetType(front_wj_->physics_joint_) != b2_revoluteJoint) { + throw YAMLException("Front wheel joint must be a revolute joint"); + } + + if (b2Joint_GetType(rear_left_wj_->physics_joint_) != b2_weldJoint) { + throw YAMLException("Rear left wheel joint must be a weld joint"); + } + + if (b2Joint_GetType(rear_right_wj_->physics_joint_) != b2_weldJoint) { + throw YAMLException("Rear right wheel joint must be a weld joint"); + } + + // enable limits for the front joint + b2RevoluteJoint_EnableLimit(front_wj_->physics_joint_, true); + + // positive joint angle goes counter clockwise from the perspective of BodyA, + // if body_ is not BodyA, we need flip the steering angle for visualization + b2Vec2 front_anchor = get_anchor(front_wj_, &invert_steering_angle_); + b2Vec2 rear_left_anchor = get_anchor(rear_left_wj_); + b2Vec2 rear_right_anchor = get_anchor(rear_right_wj_); + + // the front wheel must be at (0,0) of the body + if (fabs(front_anchor.x) > 1e-5 || fabs(front_anchor.y) > 1e-5) { + throw YAMLException( + "Front wheel joint must have its body anchored at (0, 0)"); + } + + // calculate the wheelbase and axeltrack. We also need to verify that + // the rear_center is at the perpendicular intersection between the rear axel + // and the front wheel anchor + rear_center_ = 0.5 * (rear_left_anchor + rear_right_anchor); + + // find the perpendicular intersection between line segment given by (x1, y1) + // and (x2, y2) and a point (x3, y3). + double x1 = rear_left_anchor.x, y1 = rear_left_anchor.y, + x2 = rear_right_anchor.x, y2 = rear_right_anchor.y, + x3 = front_anchor.x, y3 = front_anchor.y; + + double k = ((y2 - y1) * (x3 - x1) - (x2 - x1) * (y3 - y1)) / + ((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1)); + double x4 = x3 - k * (y2 - y1); + double y4 = y3 + k * (x2 - x1); + + // check (x4, y4) equals to rear_center_ + if (fabs(x4 - rear_center_.x) > 1e-5 || fabs(y4 - rear_center_.y) > 1e-5) { + throw YAMLException( + "The mid point between the rear wheel anchors on the body must equal " + "the perpendicular intersection between the rear axel (line segment " + "between rear anchors) and the front wheel anchor"); + } + + // track is the separation between the rear two wheels, which is simply the + // distance between the rear two wheels + axel_track_ = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); + + // wheel base is the perpendicular distance between the rear axel and the + // front wheel + wheelbase_ = sqrt((x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3)); +} + +void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { + bool publish = update_timer_.CheckUpdate(timekeeper); + + b2BodyId b2body = body_->physics_body_; + + b2Vec2 position = b2Body_GetPosition(b2body); + float angle = b2Rot_GetAngle(b2Body_GetRotation(b2body)); + + if (publish) { + // 1. get the state of the body and publish the data, + // before the tricycle physics get updated + b2Vec2 world_vel = b2Body_GetLinearVelocity(b2body); + b2Vec2 linear_vel_local = b2Body_GetLocalVector(b2body, world_vel); + float angular_vel = b2Body_GetAngularVelocity(b2body); + + ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); + ground_truth_msg_.pose.pose.position.x = position.x; + ground_truth_msg_.pose.pose.position.y = position.y; + ground_truth_msg_.pose.pose.position.z = 0; + ground_truth_msg_.pose.pose.orientation = + tf::createQuaternionMsgFromYaw(angle); + ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x; + ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y; + ground_truth_msg_.twist.twist.linear.z = 0; + ground_truth_msg_.twist.twist.angular.x = 0; + ground_truth_msg_.twist.twist.angular.y = 0; + ground_truth_msg_.twist.twist.angular.z = angular_vel; + + // add the noise to odom messages + odom_msg_.header.stamp = timekeeper.GetSimTime(); + odom_msg_.pose.pose = ground_truth_msg_.pose.pose; + odom_msg_.twist.twist = ground_truth_msg_.twist.twist; + odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); + odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); + odom_msg_.pose.pose.orientation = + tf::createQuaternionMsgFromYaw(angle + noise_gen_[2](rng_)); + odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); + odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); + odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); + + ground_truth_pub_.publish(ground_truth_msg_); + odom_pub_.publish(odom_msg_); + } + + // 2. Update the tricycle physics based on the twist command + // This is a highly simplified kinematic approximation of the response + // of the steering and drive mechanisms. + + // Equations of motion for steering (kinematics only) + // Let δ (delta) = measured steering angle + // δ_c = commanded steering angle + // Then the kinematic discrete-time equations are (1st-order approximation): + // (1) δ[t+1] = δ[t] + dδ[t+1] * dt new steering angle + // (2) dδ[t+1] = dδ[t] + d2δ[t+1] * dt new steering velocity + // (3) d2δ[t+1] = (dδ_c[t+1] - dδ[t]) / dt new steering acceleration + // (4) dδ_c[t+1] = (δ_c[t+1] - δ[t]) / dt new steering velocity command + // when it requires > 1 dt to reach δ[t+1] + // 0.0 + // otherwise + // subject to: + // |δ[t]| <= max_steer_angle_ + // |dδ[t]| <= max_steer_velocity_ + // |d2δ[t]| <= max_steer_acceleration_ + + // twist message contains the speed and angle of the front wheel + delta_command_ = twist_msg_.angular.z; // target steering angle + double theta = angle; // angle of robot in map frame + double dt = timekeeper.GetStepSize(); + + // In the simulation, the equations of motion have to be computed backwards + // (4) Update the new commanded steering velocity + // Note: Set target steer velocity = 0 rad/s to avoid overshooting, when + // it is possible to reach the commanded steering angle in 1 step + double d_delta_command = 0.0; + double delta_max_one_step = d_delta_ * d_delta_ / 2 / max_steer_acceleration_; + if (max_steer_acceleration_ == 0.0) { + delta_max_one_step = fabs(delta_command_ - theta_f_); + } + + if (fabs(delta_command_ - theta_f_) >= delta_max_one_step) { + d_delta_command = (delta_command_ - theta_f_) / dt; + } + + // Apply angular dynamics constraints + d_delta_ = angular_dynamics_.Limit(d_delta_, d_delta_command, dt); + + // (1) Update the new steering angle + theta_f_ += d_delta_ * dt; + if (max_steer_angle_ != 0.0) { + theta_f_ = DynamicsLimits::Saturate(theta_f_, -max_steer_angle_, max_steer_angle_); + } + + ROS_DEBUG_THROTTLE(0.5, + "Using new tricycle steering, " + "d_delta = %.4f, twist.x = %.4f, twist.delta = %.4f", + d_delta_, twist_msg_.linear.x, + twist_msg_.angular.z); + + // change angle of the front wheel for visualization + + b2JointId j = front_wj_->physics_joint_; + b2RevoluteJoint_EnableLimit(j, true); + if (invert_steering_angle_) { + b2RevoluteJoint_SetLimits(j, static_cast(-theta_f_), + static_cast(-theta_f_)); + } else { + b2RevoluteJoint_SetLimits(j, static_cast(theta_f_), + static_cast(theta_f_)); + } + + // calculate the desired velocity using the bicycle model in the world frame + // looking at the rear center, formulas obtained from avidbots robot systems + // confluence page + + // apply linear velocity and acceleration constraints + v_f_ = linear_dynamics_.Limit(v_f_, twist_msg_.linear.x, dt); + + double v_x = v_f_ * cos(theta_f_) * cos(theta); // x velocity in world + double v_y = v_f_ * cos(theta_f_) * sin(theta); // y velocity in world + double w = v_f_ * sin(theta_f_) / wheelbase_; // angular velocity + + // Now we would like the rear center to move at v_x, v_y, and w, since Box2D + // applies velocities at center of mass, we must use rigid body kinematics + // to transform the velocities + b2Vec2 linear_vel = {(float)v_x, (float)v_y}; + + // V_cm = V_rc + W x r_cm/rc + // velocity at center of mass equals to the velocity at the rear center plus, + // angular velocity cross product the displacement from the rear center to the + // center of mass + + // r is the vector from rear center to CM in world frame + b2Vec2 rear_center_world = b2Body_GetWorldPoint(b2body, rear_center_); + b2Vec2 com_local = b2Body_GetLocalCenterOfMass(b2body); + b2Vec2 com_world = b2Body_GetWorldPoint(b2body, com_local); + b2Vec2 r = {com_world.x - rear_center_world.x, + com_world.y - rear_center_world.y}; + b2Vec2 linear_vel_cm = {linear_vel.x + static_cast(w) * (-r.y), + linear_vel.y + static_cast(w) * r.x}; + + b2Body_SetLinearVelocity(b2body, linear_vel_cm); + + // angular velocity is the same at any point in body + b2Body_SetAngularVelocity(b2body, static_cast(w)); +} + +void TricycleDrive::TwistCallback(const geometry_msgs::Twist& msg) { + twist_msg_ = msg; +} + +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::TricycleDrive, + flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/tween.cpp b/flatland_plugins/src/tween.cpp index 0e9fc69a..bff334d4 100644 --- a/flatland_plugins/src/tween.cpp +++ b/flatland_plugins/src/tween.cpp @@ -1,288 +1,290 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name Tween.cpp - * @brief Tween plugin - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_plugins { - -std::map Tween::mode_strings_ = { - {"yoyo", Tween::ModeType_::YOYO}, - {"loop", Tween::ModeType_::LOOP}, - {"once", Tween::ModeType_::ONCE}, - {"trigger", Tween::ModeType_::TRIGGER}}; - -std::map Tween::easing_strings_ = { - {"linear", Tween::EasingType_::linear}, - {"quadraticIn", Tween::EasingType_::quadraticIn}, - {"quadraticOut", Tween::EasingType_::quadraticOut}, - {"quadraticInOut", Tween::EasingType_::quadraticInOut}, - {"cubicIn", Tween::EasingType_::cubicIn}, - {"cubicOut", Tween::EasingType_::cubicOut}, - {"cubicInOut", Tween::EasingType_::cubicInOut}, - {"quarticIn", Tween::EasingType_::quarticIn}, - {"quarticOut", Tween::EasingType_::quarticOut}, - {"quarticInOut", Tween::EasingType_::quarticInOut}, - {"quinticIn", Tween::EasingType_::quinticIn}, - {"quinticOut", Tween::EasingType_::quinticOut}, - {"quinticInOut", Tween::EasingType_::quinticInOut}, - // { "sinuisodal", Tween::EasingType_::sinuisodal }, - {"exponentialIn", Tween::EasingType_::exponentialIn}, - {"exponentialOut", Tween::EasingType_::exponentialOut}, - {"exponentialInOut", Tween::EasingType_::exponentialInOut}, - {"circularIn", Tween::EasingType_::circularIn}, - {"circularOut", Tween::EasingType_::circularOut}, - {"circularInOut", Tween::EasingType_::circularInOut}, - {"backIn", Tween::EasingType_::backIn}, - {"backOut", Tween::EasingType_::backOut}, - {"backInOut", Tween::EasingType_::backInOut}, - {"elasticIn", Tween::EasingType_::elasticIn}, - {"elasticOut", Tween::EasingType_::elasticOut}, - {"elasticInOut", Tween::EasingType_::elasticInOut}, - {"bounceIn", Tween::EasingType_::bounceIn}, - {"bounceOut", Tween::EasingType_::bounceOut}, - {"bounceInOut", Tween::EasingType_::bounceInOut}}; - -void Tween::OnInitialize(const YAML::Node& config) { - YamlReader reader(config); - std::string body_name = reader.Get("body"); - - // reciprocal, loop, or oneshot - std::string mode = reader.Get("mode", "yoyo"); - duration_ = reader.Get("duration", 1.0); - - delta_ = reader.GetPose("delta", Pose(0, 0, 0)); - - // Boolean play pause topic - std::string trigger_topic = reader.Get("trigger_topic", ""); - if (trigger_topic != "") { - trigger_sub_ = - nh_.subscribe(trigger_topic, 1, &Tween::TriggerCallback, this); - } - - body_ = GetModel()->GetBody(body_name); - if (body_ == nullptr) { - throw YAMLException("Body with name " + Q(body_name) + " does not exist"); - } - start_ = Pose(body_->physics_body_->GetPosition().x, - body_->physics_body_->GetPosition().y, - body_->physics_body_->GetAngle()); - - // Validate the mode selection - if (!Tween::mode_strings_.count(mode)) { - throw YAMLException("Mode " + mode + " does not exist"); - } - mode_ = Tween::mode_strings_.at(mode); - - tween_ = tweeny::from(0.0, 0.0, 0.0) - .to(delta_.x, delta_.y, delta_.theta) - .during((uint32)(duration_ * 1000.0)); - - Tween::EasingType_ easing_type; - std::string easing = reader.Get("easing", "linear"); - if (!Tween::easing_strings_.count(easing)) { - throw YAMLException("Mode " + mode + " does not exist"); - } - easing_type = Tween::easing_strings_.at(easing); - - // This is clumsy but because tweeny used structs for each tweening rather - // than subclasses - // I believe that this is the best way to do this - switch (easing_type) { - case Tween::EasingType_::linear: - tween_ = tween_.via(tweeny::easing::linear); - break; - case Tween::EasingType_::quadraticIn: - tween_ = tween_.via(tweeny::easing::quadraticIn); - break; - case Tween::EasingType_::quadraticOut: - tween_ = tween_.via(tweeny::easing::quadraticOut); - break; - case Tween::EasingType_::quadraticInOut: - tween_ = tween_.via(tweeny::easing::quadraticInOut); - break; - case Tween::EasingType_::cubicIn: - tween_ = tween_.via(tweeny::easing::cubicIn); - break; - case Tween::EasingType_::cubicOut: - tween_ = tween_.via(tweeny::easing::cubicOut); - break; - case Tween::EasingType_::cubicInOut: - tween_ = tween_.via(tweeny::easing::cubicInOut); - break; - case Tween::EasingType_::quarticIn: - tween_ = tween_.via(tweeny::easing::quarticIn); - break; - case Tween::EasingType_::quarticOut: - tween_ = tween_.via(tweeny::easing::quarticOut); - break; - case Tween::EasingType_::quarticInOut: - tween_ = tween_.via(tweeny::easing::quarticInOut); - break; - case Tween::EasingType_::quinticIn: - tween_ = tween_.via(tweeny::easing::quinticIn); - break; - case Tween::EasingType_::quinticOut: - tween_ = tween_.via(tweeny::easing::quinticOut); - break; - case Tween::EasingType_::quinticInOut: - tween_ = tween_.via(tweeny::easing::quinticInOut); - break; - // case Tween::EasingType_::sinuisodal: - // tween_ = tween_.via(tweeny::easing::sinuisodal); - // break; - case Tween::EasingType_::exponentialIn: - tween_ = tween_.via(tweeny::easing::exponentialIn); - break; - case Tween::EasingType_::exponentialOut: - tween_ = tween_.via(tweeny::easing::exponentialOut); - break; - case Tween::EasingType_::exponentialInOut: - tween_ = tween_.via(tweeny::easing::exponentialInOut); - break; - case Tween::EasingType_::circularIn: - tween_ = tween_.via(tweeny::easing::circularIn); - break; - case Tween::EasingType_::circularOut: - tween_ = tween_.via(tweeny::easing::circularOut); - break; - case Tween::EasingType_::circularInOut: - tween_ = tween_.via(tweeny::easing::circularInOut); - break; - case Tween::EasingType_::backIn: - tween_ = tween_.via(tweeny::easing::backIn); - break; - case Tween::EasingType_::backOut: - tween_ = tween_.via(tweeny::easing::backOut); - break; - case Tween::EasingType_::backInOut: - tween_ = tween_.via(tweeny::easing::backInOut); - break; - case Tween::EasingType_::elasticIn: - tween_ = tween_.via(tweeny::easing::elasticIn); - break; - case Tween::EasingType_::elasticOut: - tween_ = tween_.via(tweeny::easing::elasticOut); - break; - case Tween::EasingType_::elasticInOut: - tween_ = tween_.via(tweeny::easing::elasticInOut); - break; - case Tween::EasingType_::bounceIn: - tween_ = tween_.via(tweeny::easing::bounceIn); - break; - case Tween::EasingType_::bounceOut: - tween_ = tween_.via(tweeny::easing::bounceOut); - break; - case Tween::EasingType_::bounceInOut: - tween_ = tween_.via(tweeny::easing::bounceInOut); - break; - default: - throw new Exception("Unknown easing type!"); - } - - // Make sure there are no unused keys - reader.EnsureAccessedAllKeys(); - - ROS_DEBUG_NAMED("Tween", - "Initialized with params body(%p %s) " - "start ({%f,%f,%f}) " - "end ({%f,%f,%f}) " - "duration %f " - "mode: %s [%d] " - "easing: %s\n", - body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, - delta_.x, delta_.y, delta_.theta, duration_, mode.c_str(), - (int)mode_, easing.c_str()); -} - -void Tween::TriggerCallback(const std_msgs::Bool& msg) { - triggered_ = msg.data; -} - -void Tween::BeforePhysicsStep(const Timekeeper& timekeeper) { - std::array v = - tween_.step((uint32)(timekeeper.GetStepSize() * 1000.0)); - ROS_DEBUG_THROTTLE_NAMED(1.0, "Tween", "value %f,%f,%f step %f progress %f", - v[0], v[1], v[2], timekeeper.GetStepSize(), - tween_.progress()); - body_->physics_body_->SetTransform(b2Vec2(start_.x + v[0], start_.y + v[1]), - start_.theta + v[2]); - // Tell Box2D to update the AABB and check for collisions for this object - body_->physics_body_->SetAwake(true); - - // Yoyo back and forth - if (mode_ == Tween::ModeType_::YOYO) { - if (tween_.progress() >= 1.0f) { - tween_.backward(); - } else if (tween_.progress() <= 0.001f) { - tween_.forward(); - } - } - - // Teleport back in loop mode - if (mode_ == Tween::ModeType_::LOOP) { - if (tween_.progress() >= 1.0f) { - tween_.seek(0); - } - } - - // Handle external trigger - if (mode_ == Tween::ModeType_::TRIGGER) { - if (triggered_) { - tween_.forward(); - } else { - tween_.backward(); - } - } -} -} - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::Tween, flatland_server::ModelPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name Tween.cpp + * @brief Tween plugin + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_plugins { + +std::map Tween::mode_strings_ = { + {"yoyo", Tween::ModeType_::YOYO}, + {"loop", Tween::ModeType_::LOOP}, + {"once", Tween::ModeType_::ONCE}, + {"trigger", Tween::ModeType_::TRIGGER}}; + +std::map Tween::easing_strings_ = { + {"linear", Tween::EasingType_::linear}, + {"quadraticIn", Tween::EasingType_::quadraticIn}, + {"quadraticOut", Tween::EasingType_::quadraticOut}, + {"quadraticInOut", Tween::EasingType_::quadraticInOut}, + {"cubicIn", Tween::EasingType_::cubicIn}, + {"cubicOut", Tween::EasingType_::cubicOut}, + {"cubicInOut", Tween::EasingType_::cubicInOut}, + {"quarticIn", Tween::EasingType_::quarticIn}, + {"quarticOut", Tween::EasingType_::quarticOut}, + {"quarticInOut", Tween::EasingType_::quarticInOut}, + {"quinticIn", Tween::EasingType_::quinticIn}, + {"quinticOut", Tween::EasingType_::quinticOut}, + {"quinticInOut", Tween::EasingType_::quinticInOut}, + // { "sinuisodal", Tween::EasingType_::sinuisodal }, + {"exponentialIn", Tween::EasingType_::exponentialIn}, + {"exponentialOut", Tween::EasingType_::exponentialOut}, + {"exponentialInOut", Tween::EasingType_::exponentialInOut}, + {"circularIn", Tween::EasingType_::circularIn}, + {"circularOut", Tween::EasingType_::circularOut}, + {"circularInOut", Tween::EasingType_::circularInOut}, + {"backIn", Tween::EasingType_::backIn}, + {"backOut", Tween::EasingType_::backOut}, + {"backInOut", Tween::EasingType_::backInOut}, + {"elasticIn", Tween::EasingType_::elasticIn}, + {"elasticOut", Tween::EasingType_::elasticOut}, + {"elasticInOut", Tween::EasingType_::elasticInOut}, + {"bounceIn", Tween::EasingType_::bounceIn}, + {"bounceOut", Tween::EasingType_::bounceOut}, + {"bounceInOut", Tween::EasingType_::bounceInOut}}; + +void Tween::OnInitialize(const YAML::Node& config) { + YamlReader reader(config); + std::string body_name = reader.Get("body"); + + // reciprocal, loop, or oneshot + std::string mode = reader.Get("mode", "yoyo"); + duration_ = reader.Get("duration", 1.0); + + delta_ = reader.GetPose("delta", Pose(0, 0, 0)); + + // Boolean play pause topic + std::string trigger_topic = reader.Get("trigger_topic", ""); + if (trigger_topic != "") { + trigger_sub_ = + nh_.subscribe(trigger_topic, 1, &Tween::TriggerCallback, this); + } + + body_ = GetModel()->GetBody(body_name); + if (body_ == nullptr) { + throw YAMLException("Body with name " + Q(body_name) + " does not exist"); + } + start_ = Pose(b2Body_GetPosition(body_->physics_body_).x, + b2Body_GetPosition(body_->physics_body_).y, + b2Rot_GetAngle(b2Body_GetRotation(body_->physics_body_))); + + // Validate the mode selection + if (!Tween::mode_strings_.count(mode)) { + throw YAMLException("Mode " + mode + " does not exist"); + } + mode_ = Tween::mode_strings_.at(mode); + + tween_ = tweeny::from(0.0, 0.0, 0.0) + .to(delta_.x, delta_.y, delta_.theta) + .during((uint32_t)(duration_ * 1000.0)); + + Tween::EasingType_ easing_type; + std::string easing = reader.Get("easing", "linear"); + if (!Tween::easing_strings_.count(easing)) { + throw YAMLException("Mode " + mode + " does not exist"); + } + easing_type = Tween::easing_strings_.at(easing); + + // This is clumsy but because tweeny used structs for each tweening rather + // than subclasses + // I believe that this is the best way to do this + switch (easing_type) { + case Tween::EasingType_::linear: + tween_ = tween_.via(tweeny::easing::linear); + break; + case Tween::EasingType_::quadraticIn: + tween_ = tween_.via(tweeny::easing::quadraticIn); + break; + case Tween::EasingType_::quadraticOut: + tween_ = tween_.via(tweeny::easing::quadraticOut); + break; + case Tween::EasingType_::quadraticInOut: + tween_ = tween_.via(tweeny::easing::quadraticInOut); + break; + case Tween::EasingType_::cubicIn: + tween_ = tween_.via(tweeny::easing::cubicIn); + break; + case Tween::EasingType_::cubicOut: + tween_ = tween_.via(tweeny::easing::cubicOut); + break; + case Tween::EasingType_::cubicInOut: + tween_ = tween_.via(tweeny::easing::cubicInOut); + break; + case Tween::EasingType_::quarticIn: + tween_ = tween_.via(tweeny::easing::quarticIn); + break; + case Tween::EasingType_::quarticOut: + tween_ = tween_.via(tweeny::easing::quarticOut); + break; + case Tween::EasingType_::quarticInOut: + tween_ = tween_.via(tweeny::easing::quarticInOut); + break; + case Tween::EasingType_::quinticIn: + tween_ = tween_.via(tweeny::easing::quinticIn); + break; + case Tween::EasingType_::quinticOut: + tween_ = tween_.via(tweeny::easing::quinticOut); + break; + case Tween::EasingType_::quinticInOut: + tween_ = tween_.via(tweeny::easing::quinticInOut); + break; + // case Tween::EasingType_::sinuisodal: + // tween_ = tween_.via(tweeny::easing::sinuisodal); + // break; + case Tween::EasingType_::exponentialIn: + tween_ = tween_.via(tweeny::easing::exponentialIn); + break; + case Tween::EasingType_::exponentialOut: + tween_ = tween_.via(tweeny::easing::exponentialOut); + break; + case Tween::EasingType_::exponentialInOut: + tween_ = tween_.via(tweeny::easing::exponentialInOut); + break; + case Tween::EasingType_::circularIn: + tween_ = tween_.via(tweeny::easing::circularIn); + break; + case Tween::EasingType_::circularOut: + tween_ = tween_.via(tweeny::easing::circularOut); + break; + case Tween::EasingType_::circularInOut: + tween_ = tween_.via(tweeny::easing::circularInOut); + break; + case Tween::EasingType_::backIn: + tween_ = tween_.via(tweeny::easing::backIn); + break; + case Tween::EasingType_::backOut: + tween_ = tween_.via(tweeny::easing::backOut); + break; + case Tween::EasingType_::backInOut: + tween_ = tween_.via(tweeny::easing::backInOut); + break; + case Tween::EasingType_::elasticIn: + tween_ = tween_.via(tweeny::easing::elasticIn); + break; + case Tween::EasingType_::elasticOut: + tween_ = tween_.via(tweeny::easing::elasticOut); + break; + case Tween::EasingType_::elasticInOut: + tween_ = tween_.via(tweeny::easing::elasticInOut); + break; + case Tween::EasingType_::bounceIn: + tween_ = tween_.via(tweeny::easing::bounceIn); + break; + case Tween::EasingType_::bounceOut: + tween_ = tween_.via(tweeny::easing::bounceOut); + break; + case Tween::EasingType_::bounceInOut: + tween_ = tween_.via(tweeny::easing::bounceInOut); + break; + default: + throw new Exception("Unknown easing type!"); + } + + // Make sure there are no unused keys + reader.EnsureAccessedAllKeys(); + + ROS_DEBUG_NAMED("Tween", + "Initialized with params body(%p %s) " + "start ({%f,%f,%f}) " + "end ({%f,%f,%f}) " + "duration %f " + "mode: %s [%d] " + "easing: %s\n", + body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, + delta_.x, delta_.y, delta_.theta, duration_, mode.c_str(), + (int)mode_, easing.c_str()); +} + +void Tween::TriggerCallback(const std_msgs::Bool& msg) { + triggered_ = msg.data; +} + +void Tween::BeforePhysicsStep(const Timekeeper& timekeeper) { + std::array v = + tween_.step((uint32_t)(timekeeper.GetStepSize() * 1000.0)); + ROS_DEBUG_THROTTLE_NAMED(1.0, "Tween", "value %f,%f,%f step %f progress %f", + v[0], v[1], v[2], timekeeper.GetStepSize(), + tween_.progress()); + b2Vec2 new_pos = {static_cast(start_.x + v[0]), + static_cast(start_.y + v[1])}; + b2Rot new_rot = b2MakeRot(static_cast(start_.theta + v[2])); + b2Body_SetTransform(body_->physics_body_, new_pos, new_rot); + // Tell Box2D to update the AABB and check for collisions for this object + b2Body_SetAwake(body_->physics_body_, true); + + // Yoyo back and forth + if (mode_ == Tween::ModeType_::YOYO) { + if (tween_.progress() >= 1.0f) { + tween_.backward(); + } else if (tween_.progress() <= 0.001f) { + tween_.forward(); + } + } + + // Teleport back in loop mode + if (mode_ == Tween::ModeType_::LOOP) { + if (tween_.progress() >= 1.0f) { + tween_.seek(0); + } + } + + // Handle external trigger + if (mode_ == Tween::ModeType_::TRIGGER) { + if (triggered_) { + tween_.forward(); + } else { + tween_.backward(); + } + } +} +} + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::Tween, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/update_timer.cpp b/flatland_plugins/src/update_timer.cpp index 50c3728d..9706e75d 100644 --- a/flatland_plugins/src/update_timer.cpp +++ b/flatland_plugins/src/update_timer.cpp @@ -1,91 +1,91 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name update_timer.cpp - * @brief For managing plugin update rates - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -namespace flatland_plugins { - -UpdateTimer::UpdateTimer() - : period_(ros::Duration(0)), last_update_time_(ros::Time(0, 0)) {} - -void UpdateTimer::SetRate(double rate) { - if (rate == 0.0) - period_ = ros::Duration(INT32_MAX, 0); // 1000 hours is infinity right? - else - period_ = ros::Duration(1.0 / rate); -} - -bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper &timekeeper) { - if (fabs(period_.toSec()) < 1e-5) { - return true; - } - - // Naive way of keeping the update rate, will produce update rate always - // slightly below the desired rate - /* - if (now - last_update_time_ > period_) { - last_update_time_ = now; - return true; - } - return false; - */ - - // Method obtained from Hector Gazebo Plugins, works well when the step size - // is stable and close to max step size. - // hector_gazebo/hector_gazebo_plugins/include/hector_gazebo_plugins/update_timer.h - double step = timekeeper.GetMaxStepSize(); - double fraction = - fmod(timekeeper.GetSimTime().toSec() + (step / 2.0), period_.toSec()); - - if ((fraction >= 0.0) && (fraction < step)) { - last_update_time_ = timekeeper.GetSimTime(); - return true; - } - - return false; -} -}; +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name update_timer.cpp + * @brief For managing plugin update rates + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +namespace flatland_plugins { + +UpdateTimer::UpdateTimer() + : period_(ros::Duration(0)), last_update_time_(ros::Time(0, 0)) {} + +void UpdateTimer::SetRate(double rate) { + if (rate == 0.0) + period_ = ros::Duration(INT32_MAX, 0); // 1000 hours is infinity right? + else + period_ = ros::Duration(1.0 / rate); +} + +bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper &timekeeper) { + if (fabs(period_.toSec()) < 1e-5) { + return true; + } + + // Naive way of keeping the update rate, will produce update rate always + // slightly below the desired rate + /* + if (now - last_update_time_ > period_) { + last_update_time_ = now; + return true; + } + return false; + */ + + // Method obtained from Hector Gazebo Plugins, works well when the step size + // is stable and close to max step size. + // hector_gazebo/hector_gazebo_plugins/include/hector_gazebo_plugins/update_timer.h + double step = timekeeper.GetMaxStepSize(); + double fraction = + fmod(timekeeper.GetSimTime().toSec() + (step / 2.0), period_.toSec()); + + if ((fraction >= 0.0) && (fraction < step)) { + last_update_time_ = timekeeper.GetSimTime(); + return true; + } + + return false; +} +}; diff --git a/flatland_plugins/src/world_modifier.cpp b/flatland_plugins/src/world_modifier.cpp index 9829b8ce..993be877 100644 --- a/flatland_plugins/src/world_modifier.cpp +++ b/flatland_plugins/src/world_modifier.cpp @@ -1,197 +1,199 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_modifier.h - * @brief defintions for functions from world_modifier.h - * @author Arthur Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -using std::cout; -using std::endl; -using namespace std::placeholders; -using namespace flatland_server; - -namespace flatland_plugins { - -float RayTrace::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) { - // only register hit in the specified layers - if (!(fixture->GetFilterData().categoryBits & category_bits_)) { - // cout << "hit others " << endl; - return -1.0f; // return -1 to ignore this hit - } - is_hit_ = true; - fraction_ = fraction; - return fraction; -} - -WorldModifier::WorldModifier(flatland_server::World *world, - std::string layer_name, double wall_wall_dist, - bool double_wall, Pose robot_ini_pose) - : world_(world), - layer_name_(layer_name), - wall_wall_dist_(wall_wall_dist), - double_wall_(double_wall), - robot_ini_pose_(robot_ini_pose) {} - -void WorldModifier::CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, - b2EdgeShape &new_wall) { - b2Vec2 new_wall_v1; - b2Vec2 new_wall_v2; - if (d == 0) { // if distance towards the robot is 0 - ROS_FATAL_NAMED("Node", "robot start pose hit the wall!"); - } else if (d < 0) { // if on the left side - if (vertex1.x == vertex2.x) { // if it is a vertical wall - new_wall_v1.Set(vertex1.x + wall_wall_dist_, vertex1.y); - new_wall_v2.Set(vertex2.x + wall_wall_dist_, vertex2.y); - } else if (vertex1.y == vertex2.y) { // if it is a horizontal wall - new_wall_v1.Set(vertex1.x, vertex1.y + wall_wall_dist_); - new_wall_v2.Set(vertex2.x, vertex2.y + wall_wall_dist_); - } else { // if it's an angled wall - new_wall_v1.Set(vertex1.x + wall_wall_dist_, vertex1.y + wall_wall_dist_); - new_wall_v2.Set(vertex2.x + wall_wall_dist_, vertex2.y + wall_wall_dist_); - } - } else { // if on the right side - if (vertex1.x == vertex2.x) { // if it is a vertical wall - new_wall_v1.Set(vertex1.x - wall_wall_dist_, vertex1.y); - new_wall_v2.Set(vertex2.x - wall_wall_dist_, vertex2.y); - } else if (vertex1.y == vertex2.y) { // if it is a horizontal wall - new_wall_v1.Set(vertex1.x, vertex1.y - wall_wall_dist_); - new_wall_v2.Set(vertex2.x, vertex2.y - wall_wall_dist_); - } else { // if it's an angled wall - new_wall_v1.Set(vertex1.x - wall_wall_dist_, vertex1.y - wall_wall_dist_); - new_wall_v2.Set(vertex2.x - wall_wall_dist_, vertex2.y - wall_wall_dist_); - } - } - new_wall.Set(new_wall_v1, new_wall_v2); -} - -void WorldModifier::AddWall(b2EdgeShape &new_wall) { - Layer *layer = NULL; - std::vector cfr_names; - for (auto &it : world_->layers_name_map_) { - for (auto &v_it : it.first) { - if (v_it == layer_name_) { - layer = it.second; - cfr_names = it.first; - break; - } - } - } - if (layer == NULL) { - throw("no such layer name!"); - } - b2FixtureDef fixture_def; - fixture_def.shape = &new_wall; - uint16_t categoryBits = layer->cfr_->GetCategoryBits(cfr_names); - fixture_def.filter.categoryBits = categoryBits; - fixture_def.filter.maskBits = categoryBits; - - layer->body_->physics_body_->CreateFixture(&fixture_def); -} - -void WorldModifier::AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall) { - b2Vec2 old_wall_v1 = old_wall.m_vertex1; - b2Vec2 old_wall_v2 = old_wall.m_vertex2; - b2Vec2 new_wall_v1 = new_wall.m_vertex1; - b2Vec2 new_wall_v2 = new_wall.m_vertex2; - // first side - double k = - ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v1.x - old_wall_v1.x) - - (old_wall_v2.x - old_wall_v1.x) * (new_wall_v1.y - old_wall_v1.y)) / - (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + - std::pow((old_wall_v2.x - old_wall_v1.x), 2)); - double x = new_wall_v1.x - k * (old_wall_v2.y - old_wall_v1.y); - double y = new_wall_v1.y + k * (old_wall_v2.x - old_wall_v1.x); - b2EdgeShape first_wall; - first_wall.Set(new_wall_v1, b2Vec2(x, y)); - AddWall(first_wall); - - // second side - k = ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v2.x - old_wall_v1.x) - - (old_wall_v2.x - old_wall_v1.x) * (new_wall_v2.y - old_wall_v1.y)) / - (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + - std::pow((old_wall_v2.x - old_wall_v1.x), 2)); - x = new_wall_v2.x - k * (old_wall_v2.y - old_wall_v1.y); - y = new_wall_v2.y + k * (old_wall_v2.x - old_wall_v1.x); - b2EdgeShape second_wall; - second_wall.Set(new_wall_v2, b2Vec2(x, y)); - AddWall(second_wall); -} - -void WorldModifier::AddFullWall(b2EdgeShape *wall) { - b2Vec2 vertex1 = wall->m_vertex1; - b2Vec2 vertex2 = wall->m_vertex2; - double d = (robot_ini_pose_.x - vertex1.x) * (vertex2.y - vertex1.y) - - (robot_ini_pose_.y - vertex1.y) * (vertex2.x - vertex1.x); - - // add the main wall - b2EdgeShape new_wall; - CalculateNewWall(d, vertex1, vertex2, new_wall); - AddWall(new_wall); - // add the sidewall - AddSideWall(*wall, new_wall); - - if (double_wall_) { // if add walls on both side - CalculateNewWall(-d, vertex1, vertex2, new_wall); - AddWall(new_wall); - // add the sidewall - AddSideWall(*wall, new_wall); - } -} -}; // namespace \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_modifier.h + * @brief defintions for functions from world_modifier.h + * @author Arthur Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using std::cout; +using std::endl; +using namespace std::placeholders; +using namespace flatland_server; + +namespace flatland_plugins { + +float RayTraceRayCastFcn(b2ShapeId shapeId, b2Vec2 /*point*/, + b2Vec2 /*normal*/, float fraction, void *context) { + auto *rt = static_cast(context); + uint16_t category_bits = + static_cast(b2Shape_GetFilter(shapeId).categoryBits); + if (!(category_bits & rt->category_bits_)) { + return -1.0f; // ignore + } + rt->is_hit_ = true; + rt->fraction_ = fraction; + return fraction; +} + +WorldModifier::WorldModifier(flatland_server::World *world, + std::string layer_name, double wall_wall_dist, + bool double_wall, Pose robot_ini_pose) + : world_(world), + layer_name_(layer_name), + wall_wall_dist_(wall_wall_dist), + double_wall_(double_wall), + robot_ini_pose_(robot_ini_pose) {} + +void WorldModifier::CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, + b2Segment &new_wall) { + b2Vec2 new_wall_v1; + b2Vec2 new_wall_v2; + if (d == 0) { // if distance towards the robot is 0 + ROS_FATAL_NAMED("Node", "robot start pose hit the wall!"); + } else if (d < 0) { // if on the left side + if (vertex1.x == vertex2.x) { // if it is a vertical wall + new_wall_v1 = {vertex1.x + static_cast(wall_wall_dist_), vertex1.y}; + new_wall_v2 = {vertex2.x + static_cast(wall_wall_dist_), vertex2.y}; + } else if (vertex1.y == vertex2.y) { // if it is a horizontal wall + new_wall_v1 = {vertex1.x, vertex1.y + static_cast(wall_wall_dist_)}; + new_wall_v2 = {vertex2.x, vertex2.y + static_cast(wall_wall_dist_)}; + } else { // if it's an angled wall + new_wall_v1 = {vertex1.x + static_cast(wall_wall_dist_), + vertex1.y + static_cast(wall_wall_dist_)}; + new_wall_v2 = {vertex2.x + static_cast(wall_wall_dist_), + vertex2.y + static_cast(wall_wall_dist_)}; + } + } else { // if on the right side + if (vertex1.x == vertex2.x) { // if it is a vertical wall + new_wall_v1 = {vertex1.x - static_cast(wall_wall_dist_), vertex1.y}; + new_wall_v2 = {vertex2.x - static_cast(wall_wall_dist_), vertex2.y}; + } else if (vertex1.y == vertex2.y) { // if it is a horizontal wall + new_wall_v1 = {vertex1.x, vertex1.y - static_cast(wall_wall_dist_)}; + new_wall_v2 = {vertex2.x, vertex2.y - static_cast(wall_wall_dist_)}; + } else { // if it's an angled wall + new_wall_v1 = {vertex1.x - static_cast(wall_wall_dist_), + vertex1.y - static_cast(wall_wall_dist_)}; + new_wall_v2 = {vertex2.x - static_cast(wall_wall_dist_), + vertex2.y - static_cast(wall_wall_dist_)}; + } + } + new_wall = {new_wall_v1, new_wall_v2}; +} + +void WorldModifier::AddWall(b2Segment &new_wall) { + Layer *layer = NULL; + std::vector cfr_names; + for (auto &it : world_->layers_name_map_) { + for (auto &v_it : it.first) { + if (v_it == layer_name_) { + layer = it.second; + cfr_names = it.first; + break; + } + } + } + if (layer == NULL) { + throw("no such layer name!"); + } + b2ShapeDef shape_def = b2DefaultShapeDef(); + uint16_t categoryBits = layer->cfr_->GetCategoryBits(cfr_names); + shape_def.filter.categoryBits = categoryBits; + shape_def.filter.maskBits = categoryBits; + + b2CreateSegmentShape(layer->body_->physics_body_, &shape_def, &new_wall); +} + +void WorldModifier::AddSideWall(b2Segment &old_wall, b2Segment &new_wall) { + b2Vec2 old_wall_v1 = old_wall.point1; + b2Vec2 old_wall_v2 = old_wall.point2; + b2Vec2 new_wall_v1 = new_wall.point1; + b2Vec2 new_wall_v2 = new_wall.point2; + // first side + double k = + ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v1.x - old_wall_v1.x) - + (old_wall_v2.x - old_wall_v1.x) * (new_wall_v1.y - old_wall_v1.y)) / + (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + + std::pow((old_wall_v2.x - old_wall_v1.x), 2)); + double x = new_wall_v1.x - k * (old_wall_v2.y - old_wall_v1.y); + double y = new_wall_v1.y + k * (old_wall_v2.x - old_wall_v1.x); + b2Segment first_wall = {new_wall_v1, {static_cast(x), static_cast(y)}}; + AddWall(first_wall); + + // second side + k = ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v2.x - old_wall_v1.x) - + (old_wall_v2.x - old_wall_v1.x) * (new_wall_v2.y - old_wall_v1.y)) / + (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + + std::pow((old_wall_v2.x - old_wall_v1.x), 2)); + x = new_wall_v2.x - k * (old_wall_v2.y - old_wall_v1.y); + y = new_wall_v2.y + k * (old_wall_v2.x - old_wall_v1.x); + b2Segment second_wall = {new_wall_v2, {static_cast(x), static_cast(y)}}; + AddWall(second_wall); +} + +void WorldModifier::AddFullWall(b2Segment *wall) { + b2Vec2 vertex1 = wall->point1; + b2Vec2 vertex2 = wall->point2; + double d = (robot_ini_pose_.x - vertex1.x) * (vertex2.y - vertex1.y) - + (robot_ini_pose_.y - vertex1.y) * (vertex2.x - vertex1.x); + + // add the main wall + b2Segment new_wall; + CalculateNewWall(d, vertex1, vertex2, new_wall); + AddWall(new_wall); + // add the sidewall + AddSideWall(*wall, new_wall); + + if (double_wall_) { // if add walls on both side + CalculateNewWall(-d, vertex1, vertex2, new_wall); + AddWall(new_wall); + // add the sidewall + AddSideWall(*wall, new_wall); + } +} +}; // namespace diff --git a/flatland_plugins/src/world_random_wall.cpp b/flatland_plugins/src/world_random_wall.cpp index 7bc1203a..86d8c0f6 100644 --- a/flatland_plugins/src/world_random_wall.cpp +++ b/flatland_plugins/src/world_random_wall.cpp @@ -1,132 +1,140 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_random_wall.h - * @brief a simple plugin that add random walls on the field - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace flatland_server; -using std::cout; -using std::endl; -namespace flatland_plugins { -void RandomWall::OnInitialize(const YAML::Node &config) { - // read in the plugin config - YamlReader plugin_reader(config); - std::string layer_name = plugin_reader.Get("layer", ""); - unsigned int num_of_walls = - plugin_reader.Get("num_of_walls", 0); - double wall_wall_dist = plugin_reader.Get("wall_wall_dist", 1); - bool double_wall = plugin_reader.Get("double_wall", false); - std::string robot_name = plugin_reader.Get("robot_name", ""); - Layer *layer = NULL; - for (auto &it : world_->layers_name_map_) { - for (auto &v_it : it.first) { - if (v_it == layer_name) { - layer = it.second; - break; - } - } - } - if (layer == NULL) { - throw("no such layer name!"); - } - - // read in the robot location from the world.yaml - Pose robot_ini_pose; - YamlReader models_reader = - world_config_.SubnodeOpt("models", YamlReader::LIST); - if (models_reader.IsNodeNull()) { - throw("no robot specified!"); - } - for (int i = 0; i < models_reader.NodeSize(); i++) { - YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); - if (i + 1 >= models_reader.NodeSize() && - reader.Get("name") != robot_name) { - throw("cannot find specified robot name of " + robot_name); - } - if (reader.Get("name") == robot_name) { - robot_ini_pose = reader.Get("pose", Pose(0, 0, 0)); - b2Transform tran = layer->body_->physics_body_->GetTransform(); - b2Vec2 ini_pose = - b2MulT(tran, b2Vec2(robot_ini_pose.x, robot_ini_pose.y)); - robot_ini_pose.x = ini_pose.x; - robot_ini_pose.y = ini_pose.y; - break; - } - } - // create the world modifiyer - cout << "robot location read" << endl; - WorldModifier modifier(world_, layer_name, wall_wall_dist, double_wall, - robot_ini_pose); - - // get all walls - std::vector Wall_List; - for (b2Fixture *f = layer->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { - Wall_List.push_back(static_cast(f->GetShape())); - } - std::srand(std::time(0)); - std::random_shuffle(Wall_List.begin(), Wall_List.end()); - try { - for (unsigned int i = 0; i < num_of_walls; i++) { - modifier.AddFullWall(Wall_List[i]); - } - } catch (std::string e) { - throw e; - } -} -}; // namespace - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::RandomWall, - flatland_server::WorldPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_random_wall.h + * @brief a simple plugin that add random walls on the field + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flatland_server; +using std::cout; +using std::endl; +namespace flatland_plugins { +void RandomWall::OnInitialize(const YAML::Node &config) { + // read in the plugin config + YamlReader plugin_reader(config); + std::string layer_name = plugin_reader.Get("layer", ""); + unsigned int num_of_walls = + plugin_reader.Get("num_of_walls", 0); + double wall_wall_dist = plugin_reader.Get("wall_wall_dist", 1); + bool double_wall = plugin_reader.Get("double_wall", false); + std::string robot_name = plugin_reader.Get("robot_name", ""); + Layer *layer = NULL; + for (auto &it : world_->layers_name_map_) { + for (auto &v_it : it.first) { + if (v_it == layer_name) { + layer = it.second; + break; + } + } + } + if (layer == NULL) { + throw("no such layer name!"); + } + + // read in the robot location from the world.yaml + Pose robot_ini_pose; + YamlReader models_reader = + world_config_.SubnodeOpt("models", YamlReader::LIST); + if (models_reader.IsNodeNull()) { + throw("no robot specified!"); + } + for (int i = 0; i < models_reader.NodeSize(); i++) { + YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); + if (i + 1 >= models_reader.NodeSize() && + reader.Get("name") != robot_name) { + throw("cannot find specified robot name of " + robot_name); + } + if (reader.Get("name") == robot_name) { + robot_ini_pose = reader.Get("pose", Pose(0, 0, 0)); + b2Transform tran = b2Body_GetTransform(layer->body_->physics_body_); + b2Vec2 ini_b2 = {static_cast(robot_ini_pose.x), + static_cast(robot_ini_pose.y)}; + b2Vec2 ini_pose = b2InvTransformPoint(tran, ini_b2); + robot_ini_pose.x = ini_pose.x; + robot_ini_pose.y = ini_pose.y; + break; + } + } + // create the world modifiyer + cout << "robot location read" << endl; + WorldModifier modifier(world_, layer_name, wall_wall_dist, double_wall, + robot_ini_pose); + + // get all walls + // Collect all segment shapes from the layer body + int shape_count = b2Body_GetShapeCount(layer->body_->physics_body_); + std::vector all_shapes(shape_count); + b2Body_GetShapes(layer->body_->physics_body_, all_shapes.data(), shape_count); + + std::vector wall_segments; + for (const auto &sid : all_shapes) { + if (b2Shape_GetType(sid) == b2_segmentShape) { + wall_segments.push_back(b2Shape_GetSegment(sid)); + } + } + + std::srand(std::time(0)); + std::random_shuffle(wall_segments.begin(), wall_segments.end()); + try { + for (unsigned int i = 0; i < num_of_walls && i < wall_segments.size(); i++) { + modifier.AddFullWall(&wall_segments[i]); + } + } catch (std::string e) { + throw e; + } +} +}; // namespace + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::RandomWall, + flatland_server::WorldPlugin) diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 03738880..15bf71b2 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -1,311 +1,311 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name bumper_test.cpp - * @brief test bumper plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; -using namespace flatland_plugins; -using namespace flatland_msgs; - -class BumperPluginTest : public ::testing::Test { - public: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - flatland_msgs::Collisions msg1, msg2; - World* w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { - if (std::isinf(n1) && std::isinf(n2)) { - return true; - } - - if (std::isnan(n1) && std::isnan(n2)) { - return true; - } - - bool ret = fabs(n1 - n2) < epsilon; - return ret; - } - - bool StringEq(const std::string& name, const std::string& actual, - const std::string& expected) { - if (actual != expected) { - printf("%s Actual:%s != Expected:%s\n", name.c_str(), actual.c_str(), - expected.c_str()); - return false; - } - return true; - } - - bool Vector2Eq(const std::string& name, const Vector2& actual, - const std::pair expected) { - if (!fltcmp(actual.x, expected.first) || - !fltcmp(actual.y, expected.second)) { - printf("%s Actual:(%f,%f) != Expected:(%f,%f)\n", name.c_str(), actual.x, - actual.y, expected.first, expected.second); - return false; - } - return true; - } - - bool CollisionsEq(const Collisions& collisions, const std::string& frame_id, - int num_collisions) { - if (!StringEq("frame_id", collisions.header.frame_id, frame_id)) - return false; - - if (num_collisions != collisions.collisions.size()) { - printf("Num collisions Actual:%lu != Expected:%d\n", - collisions.collisions.size(), num_collisions); - return false; - } - - return true; - } - - // check the received scan data is as expected - bool CollisionEq(const Collision& collision, const std::string& entity_A, - const std::string& body_A, const std::string& entity_B, - const std::string& body_B, int return_size, - const std::pair& normal) { - if (!StringEq("entity_A", collision.entity_A, entity_A)) return false; - if (!StringEq("body_A", collision.body_A, body_A)) return false; - if (!StringEq("entity_B", collision.entity_B, entity_B)) return false; - if (!StringEq("body_B", collision.body_B, body_B)) return false; - - if (!(collision.magnitude_forces.size() <= 2 && - collision.contact_positions.size() <= 2 && - collision.contact_normals.size() <= 2 && - collision.magnitude_forces.size() == return_size && - collision.contact_positions.size() == return_size && - collision.contact_normals.size() == return_size)) { - printf( - "Vector sizes are expected to be all the same and have sizes %d, " - "magnitude_forces=%lu contact_positions=%lu contact_normals=%lu\n", - return_size, collision.magnitude_forces.size(), - collision.contact_positions.size(), collision.contact_normals.size()); - return false; - } - - for (unsigned int i = 0; i < return_size; i++) { - std::string idx = "[" + std::to_string(i) + "]"; - - if (collision.magnitude_forces[i] <= 0) { - printf("forces%s Actual:%f != Expected: >0\n", idx.c_str(), - collision.magnitude_forces[i]); - return false; - } - - if (!Vector2Eq("normals" + idx, collision.contact_normals[i], normal)) { - return false; - } - } - return true; - } - - void CollisionCb_A(const flatland_msgs::Collisions& msg) { msg1 = msg; } - - void CollisionCb_B(const flatland_msgs::Collisions& msg) { msg2 = msg; } - - void SpinRos(float hz, int iterations) { - ros::WallRate rate(hz); - for (unsigned int i = 0; i < iterations; i++) { - ros::spinOnce(); - rate.sleep(); - } - } -}; - -/** - * Test the bumper plugin for a given model and plugin configuration - */ -TEST_F(BumperPluginTest, collision_test) { - world_yaml = - this_file_dir / fs::path("bumper_tests/collision_test/world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(0.01); - w = World::MakeWorld(world_yaml.string()); - - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; - BumperPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("collisions", 1, &BumperPluginTest::CollisionCb_A, obj); - sub_2 = - nh.subscribe("collisions_B", 1, &BumperPluginTest::CollisionCb_B, obj); - - Bumper* p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - - Body* b0 = p->GetModel()->bodies_[0]; - Body* b1 = p->GetModel()->bodies_[1]; - - // check that there are no collision at the begining - for (unsigned int i = 0; i < 100; i++) { - w->Update(timekeeper); - ros::spinOnce(); - } - SpinRos(500, 10); // make sure the messages gets through - - // check time is not zero to make sure message is received - ASSERT_NE(msg1.header.stamp, ros::Time(0, 0)); - ASSERT_NE(msg2.header.stamp, ros::Time(0, 0)); - - // step 15 time which makes the body move 1.5 meters, will make base_link_1 - // collide, but not base_link_2, not that base_link_1's fixture is a sensor - for (unsigned int i = 0; i < 150; i++) { - // Box2D needs velocity to be set every time step to ensure things are - // moving at the desired velocity - b0->physics_body_->SetLinearVelocity(b2Vec2(1, 0.0)); - w->Update(timekeeper); - ros::spinOnce(); - } - SpinRos(500, 10); // makes sure the ros message gets through - - ASSERT_TRUE(CollisionsEq(msg1, "map", 1)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); - EXPECT_TRUE(CollisionsEq(msg2, "world", 0)); - - // step another 5 times which moves 0.5 meters colliding base_link_2 as well - for (unsigned int i = 0; i < 50; i++) { - b0->physics_body_->SetLinearVelocity(b2Vec2(1, 0.0)); - w->Update(timekeeper); - ros::spinOnce(); - } - SpinRos(500, 10); - ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); - EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {1, 0})); - ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); - EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {1, 0})); - - // Now move backward far away from the wall, there collisions should clear - for (unsigned int i = 0; i < 300; i++) { - b0->physics_body_->SetLinearVelocity(b2Vec2(-1, 0.0)); - w->Update(timekeeper); - ros::spinOnce(); - } - SpinRos(500, 10); - - EXPECT_TRUE(CollisionsEq(msg1, "map", 0)); - EXPECT_TRUE(CollisionsEq(msg2, "world", 0)); - - // Teleport the body to the other side of the wall, try hitting the wall from - // the other direction, the collision normal vector should be flipped now - b0->physics_body_->SetTransform(b2Vec2(4, 0), 0); - b1->physics_body_->SetTransform(b2Vec2(4, 0), 0); - - for (unsigned int i = 0; i < 300; i++) { - b0->physics_body_->SetLinearVelocity(b2Vec2(-1, 0.0)); - w->Update(timekeeper); - ros::spinOnce(); - } - SpinRos(500, 10); - - ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); - EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {-1, 0})); - ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); - EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {-1, 0})); - // w->DebugVisualize(); - // DebugVisualization::Get().Publish(); - // ros::spin(); -} - -/** - * Test with a invalid body specified in the exclude list - */ -TEST_F(BumperPluginTest, invalid_A) { - world_yaml = this_file_dir / fs::path("bumper_tests/invalid_A/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { - std::cmatch match; - std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "bumper_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name bumper_test.cpp + * @brief test bumper plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; +using namespace flatland_plugins; +using namespace flatland_msgs; + +class BumperPluginTest : public ::testing::Test { + public: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + flatland_msgs::Collisions msg1, msg2; + World* w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { + if (std::isinf(n1) && std::isinf(n2)) { + return true; + } + + if (std::isnan(n1) && std::isnan(n2)) { + return true; + } + + bool ret = fabs(n1 - n2) < epsilon; + return ret; + } + + bool StringEq(const std::string& name, const std::string& actual, + const std::string& expected) { + if (actual != expected) { + printf("%s Actual:%s != Expected:%s\n", name.c_str(), actual.c_str(), + expected.c_str()); + return false; + } + return true; + } + + bool Vector2Eq(const std::string& name, const Vector2& actual, + const std::pair expected) { + if (!fltcmp(actual.x, expected.first) || + !fltcmp(actual.y, expected.second)) { + printf("%s Actual:(%f,%f) != Expected:(%f,%f)\n", name.c_str(), actual.x, + actual.y, expected.first, expected.second); + return false; + } + return true; + } + + bool CollisionsEq(const Collisions& collisions, const std::string& frame_id, + int num_collisions) { + if (!StringEq("frame_id", collisions.header.frame_id, frame_id)) + return false; + + if (num_collisions != collisions.collisions.size()) { + printf("Num collisions Actual:%lu != Expected:%d\n", + collisions.collisions.size(), num_collisions); + return false; + } + + return true; + } + + // check the received scan data is as expected + bool CollisionEq(const Collision& collision, const std::string& entity_A, + const std::string& body_A, const std::string& entity_B, + const std::string& body_B, int return_size, + const std::pair& normal) { + if (!StringEq("entity_A", collision.entity_A, entity_A)) return false; + if (!StringEq("body_A", collision.body_A, body_A)) return false; + if (!StringEq("entity_B", collision.entity_B, entity_B)) return false; + if (!StringEq("body_B", collision.body_B, body_B)) return false; + + if (!(collision.magnitude_forces.size() <= 2 && + collision.contact_positions.size() <= 2 && + collision.contact_normals.size() <= 2 && + collision.magnitude_forces.size() == return_size && + collision.contact_positions.size() == return_size && + collision.contact_normals.size() == return_size)) { + printf( + "Vector sizes are expected to be all the same and have sizes %d, " + "magnitude_forces=%lu contact_positions=%lu contact_normals=%lu\n", + return_size, collision.magnitude_forces.size(), + collision.contact_positions.size(), collision.contact_normals.size()); + return false; + } + + for (unsigned int i = 0; i < return_size; i++) { + std::string idx = "[" + std::to_string(i) + "]"; + + if (collision.magnitude_forces[i] <= 0) { + printf("forces%s Actual:%f != Expected: >0\n", idx.c_str(), + collision.magnitude_forces[i]); + return false; + } + + if (!Vector2Eq("normals" + idx, collision.contact_normals[i], normal)) { + return false; + } + } + return true; + } + + void CollisionCb_A(const flatland_msgs::Collisions& msg) { msg1 = msg; } + + void CollisionCb_B(const flatland_msgs::Collisions& msg) { msg2 = msg; } + + void SpinRos(float hz, int iterations) { + ros::WallRate rate(hz); + for (unsigned int i = 0; i < iterations; i++) { + ros::spinOnce(); + rate.sleep(); + } + } +}; + +/** + * Test the bumper plugin for a given model and plugin configuration + */ +TEST_F(BumperPluginTest, collision_test) { + world_yaml = + this_file_dir / fs::path("bumper_tests/collision_test/world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(0.01); + w = World::MakeWorld(world_yaml.string()); + + ros::NodeHandle nh; + ros::Subscriber sub_1, sub_2, sub_3; + BumperPluginTest* obj = dynamic_cast(this); + sub_1 = nh.subscribe("collisions", 1, &BumperPluginTest::CollisionCb_A, obj); + sub_2 = + nh.subscribe("collisions_B", 1, &BumperPluginTest::CollisionCb_B, obj); + + Bumper* p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + + Body* b0 = p->GetModel()->bodies_[0]; + Body* b1 = p->GetModel()->bodies_[1]; + + // check that there are no collision at the begining + for (unsigned int i = 0; i < 100; i++) { + w->Update(timekeeper); + ros::spinOnce(); + } + SpinRos(500, 10); // make sure the messages gets through + + // check time is not zero to make sure message is received + ASSERT_NE(msg1.header.stamp, ros::Time(0, 0)); + ASSERT_NE(msg2.header.stamp, ros::Time(0, 0)); + + // step 15 time which makes the body move 1.5 meters, will make base_link_1 + // collide, but not base_link_2, not that base_link_1's fixture is a sensor + for (unsigned int i = 0; i < 150; i++) { + // Box2D needs velocity to be set every time step to ensure things are + // moving at the desired velocity + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{1, 0.0f}); + w->Update(timekeeper); + ros::spinOnce(); + } + SpinRos(500, 10); // makes sure the ros message gets through + + ASSERT_TRUE(CollisionsEq(msg1, "map", 1)); + EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", + "layer_1", "layer_1", 0, {})); + EXPECT_TRUE(CollisionsEq(msg2, "world", 0)); + + // step another 5 times which moves 0.5 meters colliding base_link_2 as well + for (unsigned int i = 0; i < 50; i++) { + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{1, 0.0f}); + w->Update(timekeeper); + ros::spinOnce(); + } + SpinRos(500, 10); + ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); + EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", + "layer_1", "layer_1", 0, {})); + EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", + "layer_1", "layer_1", 1, {1, 0})); + ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); + EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", + "layer_1", "layer_1", 1, {1, 0})); + + // Now move backward far away from the wall, there collisions should clear + for (unsigned int i = 0; i < 300; i++) { + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{-1, 0.0f}); + w->Update(timekeeper); + ros::spinOnce(); + } + SpinRos(500, 10); + + EXPECT_TRUE(CollisionsEq(msg1, "map", 0)); + EXPECT_TRUE(CollisionsEq(msg2, "world", 0)); + + // Teleport the body to the other side of the wall, try hitting the wall from + // the other direction, the collision normal vector should be flipped now + b2Body_SetTransform(b0->physics_body_, b2Vec2{4, 0}, b2MakeRot(0)); + b2Body_SetTransform(b1->physics_body_, b2Vec2{4, 0}, b2MakeRot(0)); + + for (unsigned int i = 0; i < 300; i++) { + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{-1, 0.0f}); + w->Update(timekeeper); + ros::spinOnce(); + } + SpinRos(500, 10); + + ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); + EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", + "layer_1", "layer_1", 0, {})); + EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", + "layer_1", "layer_1", 1, {-1, 0})); + ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); + EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", + "layer_1", "layer_1", 1, {-1, 0})); + // w->DebugVisualize(); + // DebugVisualization::Get().Publish(); + // ros::spin(); +} + +/** + * Test with a invalid body specified in the exclude list + */ +TEST_F(BumperPluginTest, invalid_A) { + world_yaml = this_file_dir / fs::path("bumper_tests/invalid_A/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException& e) { + std::cmatch match; + std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception& e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "bumper_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/bumper_tests/collision_test/world.yaml b/flatland_plugins/test/bumper_tests/collision_test/world.yaml index c572968a..008e3815 100644 --- a/flatland_plugins/test/bumper_tests/collision_test/world.yaml +++ b/flatland_plugins/test/bumper_tests/collision_test/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/bumper_tests/collision_test/world_plugins.yaml b/flatland_plugins/test/bumper_tests/collision_test/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/bumper_tests/collision_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/bumper_tests/invalid_A/world.yaml b/flatland_plugins/test/bumper_tests/invalid_A/world.yaml index e8106aca..d39fb5e1 100644 --- a/flatland_plugins/test/bumper_tests/invalid_A/world.yaml +++ b/flatland_plugins/test/bumper_tests/invalid_A/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../collision_test/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/bumper_tests/invalid_A/world_plugins.yaml b/flatland_plugins/test/bumper_tests/invalid_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/bumper_tests/invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/diff_drive_test.cpp b/flatland_plugins/test/diff_drive_test.cpp index 36192918..b3a62f08 100644 --- a/flatland_plugins/test/diff_drive_test.cpp +++ b/flatland_plugins/test/diff_drive_test.cpp @@ -1,70 +1,70 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name diff_drive_test.cpp - * @brief test diff drive plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -TEST(DiffDrivePluginTest, load_test) { - pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); - - try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::DiffDrive"); - } catch (pluginlib::PluginlibException& e) { - FAIL() << "Failed to load diff drive Drive plugin. " << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "diff_drive_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name diff_drive_test.cpp + * @brief test diff drive plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +TEST(DiffDrivePluginTest, load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::DiffDrive"); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load diff drive Drive plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "diff_drive_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/diff_drive_test.test b/flatland_plugins/test/diff_drive_test.test index 1c460beb..de640e80 100644 --- a/flatland_plugins/test/diff_drive_test.test +++ b/flatland_plugins/test/diff_drive_test.test @@ -5,5 +5,5 @@ This file is used so that rosmaster is running when the test is executed, in order to test publish/subscribe --> - + \ No newline at end of file diff --git a/flatland_plugins/test/dynamics_limits_test.cpp b/flatland_plugins/test/dynamics_limits_test.cpp index 18c31013..2e5ab187 100644 --- a/flatland_plugins/test/dynamics_limits_test.cpp +++ b/flatland_plugins/test/dynamics_limits_test.cpp @@ -1,392 +1,392 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2021 Avidbots Corp. - * @name dynamics_limits_test.cpp - * @brief Test dynamics limits class - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -using namespace flatland_plugins; - -/** - * Test loading blank parameters - */ -TEST(DynamicsLimitsTest, test_Configure_blank) { - auto dynamics_limits = DynamicsLimits(); - - // Ensure defaults are zero - EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); -} - - -/** - * Test loading blank parameters - */ -TEST(DynamicsLimitsTest, test_Configure_empty_yaml) { - auto dynamics_limits = DynamicsLimits(); - YAML::Node config = YAML::Node(); - dynamics_limits.Configure(config); - - // Ensure defaults are zero - EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); -} - - -/** - * Test loading acceleration+velocity parameters - */ -TEST(DynamicsLimitsTest, test_Configure_two_params) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 0.1; - config["velocity_limit"] = 2.0; - dynamics_limits.Configure(config); - - // Ensure defaults are zero - EXPECT_NEAR(0.1, dynamics_limits.acceleration_limit_, 1e-3); - // deceleration cap defaults to accleration cap if not set - EXPECT_NEAR(0.1, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(2.0, dynamics_limits.velocity_limit_, 1e-3); -} - -/** - * Test loading acceleration, deceleration, velocity parameters - */ -TEST(DynamicsLimitsTest, test_Configure_three_params) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 0.1; - config["deceleration_limit"] = 0.2; - config["velocity_limit"] = 2.0; - dynamics_limits.Configure(config); - - // Ensure params are correct - EXPECT_NEAR(0.1, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(0.2, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(2.0, dynamics_limits.velocity_limit_, 1e-3); -} - -// todo: test Saturate - -/** - * Test Limit function without dynamics limits in place - */ -TEST(DynamicsLimitsTest, test_Limit_noop) { - auto dynamics_limits = DynamicsLimits(); - - double result; - result = dynamics_limits.Limit(0, 1.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(1.0, 1.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(0.1, 1.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(-100, 1.0, 0.1); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(-100, -1.0, 0.1); - EXPECT_NEAR(-1.0, result, 1e-3); - result = dynamics_limits.Limit(-100, -21.0, 0.1); - EXPECT_NEAR(-21.0, result, 1e-3); -} - - -/** - * Test Limit function - */ -TEST(DynamicsLimitsTest, test_Limit_velocity) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["velocity_limit"] = 1.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(1.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(0, 1.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(1.0, 1.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(0, 2.0, 0.05); - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(0, -1.0, 0.1); - EXPECT_NEAR(-1.0, result, 1e-3); - result = dynamics_limits.Limit(0, -0.5, 0.1); - EXPECT_NEAR(-0.5, result, 1e-3); - result = dynamics_limits.Limit(1.0, -0.5, 0.1); - EXPECT_NEAR(-0.5, result, 1e-3); - result = dynamics_limits.Limit(-100, -1.0, 0.1); - EXPECT_NEAR(-1.0, result, 1e-3); - result = dynamics_limits.Limit(0, -3.0, 0.1); - EXPECT_NEAR(-1.0, result, 1e-3); -} - - -/** - * Test Limit function wrt. acceleration - */ -TEST(DynamicsLimitsTest, test_Limit_acceleration) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 9.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(9.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(0.45, result, 1e-3); - result = dynamics_limits.Limit(1.0, 0.0, 0.05); // Try to decelerate at -20m/s/s, when the limit is 9 - EXPECT_NEAR(0.55, result, 1e-3); - result = dynamics_limits.Limit(10.0, 0.0, 0.05); // Try to decelerate at -200m/s/s, when the limit is 9 - EXPECT_NEAR(9.55, result, 1e-3); - result = dynamics_limits.Limit(-10.0, 0.0, 0.05); // Try to decelerate at 200m/s/s, when the limit is 9 - EXPECT_NEAR(-9.55, result, 1e-3); - result = dynamics_limits.Limit(5, -5, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(4.55, result, 1e-3); - result = dynamics_limits.Limit(-3, -2, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(-2.55, result, 1e-3); -} - -TEST(DynamicsLimitsTest, test_Saturate) { - auto dynamics_limits = DynamicsLimits(); - double result; - // Useless, strict bounds - result = DynamicsLimits::Saturate(0, 0, 0); // Bound between 0 and 0, 0 - EXPECT_NEAR(0, result, 1e-3); - result = DynamicsLimits::Saturate(1, 0, 0); // Bound between 1 and 0, 0 - EXPECT_NEAR(0, result, 1e-3); - result = DynamicsLimits::Saturate(-1, 0, 0); // Bound between -1 and 0, 0 - EXPECT_NEAR(0, result, 1e-3); - - // Valid bounds - result = DynamicsLimits::Saturate(0, -1.1, 2.2); - EXPECT_NEAR(0, result, 1e-3); - result = DynamicsLimits::Saturate(1, -1.1, 2.2); - EXPECT_NEAR(1, result, 1e-3); - result = DynamicsLimits::Saturate(-1, -1.1, 2.2); - EXPECT_NEAR(-1, result, 1e-3); - result = DynamicsLimits::Saturate(3, -1.1, 2.2); - EXPECT_NEAR(2.2, result, 1e-3); - result = DynamicsLimits::Saturate(-3, -1.1, 2.2); - EXPECT_NEAR(-1.1, result, 1e-3); - - // Invalid bounds, input not changed - result = DynamicsLimits::Saturate(0, 1.1, -2.2); - EXPECT_NEAR(0, result, 1e-3); - result = DynamicsLimits::Saturate(1, 1.1, -2.2); - EXPECT_NEAR(1, result, 1e-3); - result = DynamicsLimits::Saturate(-1, 1.1, -2.2); - EXPECT_NEAR(-1, result, 1e-3); - result = DynamicsLimits::Saturate(3, 1.1, -2.2); - EXPECT_NEAR(3, result, 1e-3); - result = DynamicsLimits::Saturate(-3, 1.1, -2.2); - EXPECT_NEAR(-3, result, 1e-3); -} - -/** - * Test Limit function wrt. acceleration and decelleration - */ -TEST(DynamicsLimitsTest, test_Limit_deceleration) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 1.0; - config["deceleration_limit"] = 9.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 1 (acceleration) - EXPECT_NEAR(0.05, result, 1e-3); - result = dynamics_limits.Limit(0.97, 1.0, 0.05); // Try to accelerate at 1m/s/s, when the limit is 1 - EXPECT_NEAR(1.0, result, 1e-3); - result = dynamics_limits.Limit(1.0, 0.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(0.55, result, 1e-3); - result = dynamics_limits.Limit(-1.0, -2.0, 0.05); // Try to acccelerate at 20m/s/s, when the limit is 1 - EXPECT_NEAR(-1.05, result, 1e-3); - result = dynamics_limits.Limit(-2.0, -1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(-1.55, result, 1e-3); - result = dynamics_limits.Limit(-2.0, 1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(-1.55, result, 1e-3); - result = dynamics_limits.Limit(-1.0, 2.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(-0.55, result, 1e-3); - result = dynamics_limits.Limit(1.0, 3.0, 0.05); // Try to acccelerate at 20m/s/s, when the limit is 1 - EXPECT_NEAR(1.05, result, 1e-3); - result = dynamics_limits.Limit(3.0, -1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 - EXPECT_NEAR(2.55, result, 1e-3); -} - -/** - * Test Limit function wrt. acceleration and decelleration - */ -TEST(DynamicsLimitsTest, test_Limit_zero_crossing) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 1.0; - config["deceleration_limit"] = 2.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(2.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep - EXPECT_NEAR(1.9, result, 1e-3); - - result = dynamics_limits.Limit(0.08, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep - // the deceletion from 0.08 -> 0m/s is done at 2m/s/s, taking 0.04s, 80% of the timestep - // then the acceleration from 0 -> -1 has the accleration constraints applied given the remainder of the timestep (0.01s) - // resulting in an effective acceleration limit of 1.8m/s/s over the timestep - EXPECT_NEAR(-0.01, result, 1e-3); -} - - -/** - * Test Limit function wrt. acceleration and decelleration corner case around zero crossing - */ -TEST(DynamicsLimitsTest, test_Limit_zero_crossing_corner_case1) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 0.0; - config["deceleration_limit"] = 2.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(2.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep - EXPECT_NEAR(1.9, result, 1e-3); - - result = dynamics_limits.Limit(0.08, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep - // The first 80% decelerates at 2m/s/s, then the remaining 20% of the timestep accelerates from 0 to -1m/s without acceleration limit - EXPECT_NEAR(-1.0, result, 1e-3); -} - -/** - * Test Limit function wrt. acceleration and decelleration corner case around zero crossing - */ -TEST(DynamicsLimitsTest, test_Limit_zero_crossing_corner_case2) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 2.0; - config["deceleration_limit"] = 0.0; - dynamics_limits.Configure(config); - - EXPECT_NEAR(2.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep - EXPECT_NEAR(0.0, result, 1e-3); - - result = dynamics_limits.Limit(1.0, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep (no limit) - // The portion from 1m/s to 0m/s has no limit, however the timestep is used to accelerate under limits - // 0.05 sec at -2m/s/s acceleration = -0.1m/s final velocity - EXPECT_NEAR(-0.1, result, 1e-3); -} - -/** - * Test Limit function wrt. acceleration asnd decelleration - */ -TEST(DynamicsLimitsTest, test_Limit_acceleration_and_velocity) { - auto dynamics_limits = DynamicsLimits(); - - YAML::Node config = YAML::Node(); - config["acceleration_limit"] = 1.0; - config["deceleration_limit"] = 9.0; - config["velocity_limit"] = 0.5; - dynamics_limits.Configure(config); - - EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); - EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); - EXPECT_NEAR(0.5, dynamics_limits.velocity_limit_, 1e-3); - - double result; - result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 1 (acceleration) - EXPECT_NEAR(0.05, result, 1e-3); - - result = dynamics_limits.Limit(0.48, 1.48, 0.05); // Same, but hit the velocity limit - EXPECT_NEAR(0.5, result, 1e-3); - - result = dynamics_limits.Limit(1.0, 2.0, 0.05); // Same, but we're already over the velocity limit - // The invalid input temporarily results in invalid output, because we have to voilate either decelleration or velocity constraints - // In this case, we violate velocity constraints - EXPECT_NEAR(0.55, result, 1e-3); - - result = dynamics_limits.Limit(0.55, 2.0, 0.05); // Same, but we're already over the velocity limit, - // but this time we can achieve velocity constraints without violating decelleration constraints - EXPECT_NEAR(0.5, result, 1e-3); -} - - - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2021 Avidbots Corp. + * @name dynamics_limits_test.cpp + * @brief Test dynamics limits class + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +using namespace flatland_plugins; + +/** + * Test loading blank parameters + */ +TEST(DynamicsLimitsTest, test_Configure_blank) { + auto dynamics_limits = DynamicsLimits(); + + // Ensure defaults are zero + EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); +} + + +/** + * Test loading blank parameters + */ +TEST(DynamicsLimitsTest, test_Configure_empty_yaml) { + auto dynamics_limits = DynamicsLimits(); + YAML::Node config = YAML::Node(); + dynamics_limits.Configure(config); + + // Ensure defaults are zero + EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); +} + + +/** + * Test loading acceleration+velocity parameters + */ +TEST(DynamicsLimitsTest, test_Configure_two_params) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 0.1; + config["velocity_limit"] = 2.0; + dynamics_limits.Configure(config); + + // Ensure defaults are zero + EXPECT_NEAR(0.1, dynamics_limits.acceleration_limit_, 1e-3); + // deceleration cap defaults to accleration cap if not set + EXPECT_NEAR(0.1, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(2.0, dynamics_limits.velocity_limit_, 1e-3); +} + +/** + * Test loading acceleration, deceleration, velocity parameters + */ +TEST(DynamicsLimitsTest, test_Configure_three_params) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 0.1; + config["deceleration_limit"] = 0.2; + config["velocity_limit"] = 2.0; + dynamics_limits.Configure(config); + + // Ensure params are correct + EXPECT_NEAR(0.1, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(0.2, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(2.0, dynamics_limits.velocity_limit_, 1e-3); +} + +// todo: test Saturate + +/** + * Test Limit function without dynamics limits in place + */ +TEST(DynamicsLimitsTest, test_Limit_noop) { + auto dynamics_limits = DynamicsLimits(); + + double result; + result = dynamics_limits.Limit(0, 1.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(1.0, 1.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(0.1, 1.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(-100, 1.0, 0.1); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(-100, -1.0, 0.1); + EXPECT_NEAR(-1.0, result, 1e-3); + result = dynamics_limits.Limit(-100, -21.0, 0.1); + EXPECT_NEAR(-21.0, result, 1e-3); +} + + +/** + * Test Limit function + */ +TEST(DynamicsLimitsTest, test_Limit_velocity) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["velocity_limit"] = 1.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(1.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(0, 1.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(1.0, 1.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(0, 2.0, 0.05); + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(0, -1.0, 0.1); + EXPECT_NEAR(-1.0, result, 1e-3); + result = dynamics_limits.Limit(0, -0.5, 0.1); + EXPECT_NEAR(-0.5, result, 1e-3); + result = dynamics_limits.Limit(1.0, -0.5, 0.1); + EXPECT_NEAR(-0.5, result, 1e-3); + result = dynamics_limits.Limit(-100, -1.0, 0.1); + EXPECT_NEAR(-1.0, result, 1e-3); + result = dynamics_limits.Limit(0, -3.0, 0.1); + EXPECT_NEAR(-1.0, result, 1e-3); +} + + +/** + * Test Limit function wrt. acceleration + */ +TEST(DynamicsLimitsTest, test_Limit_acceleration) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 9.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(9.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(0.45, result, 1e-3); + result = dynamics_limits.Limit(1.0, 0.0, 0.05); // Try to decelerate at -20m/s/s, when the limit is 9 + EXPECT_NEAR(0.55, result, 1e-3); + result = dynamics_limits.Limit(10.0, 0.0, 0.05); // Try to decelerate at -200m/s/s, when the limit is 9 + EXPECT_NEAR(9.55, result, 1e-3); + result = dynamics_limits.Limit(-10.0, 0.0, 0.05); // Try to decelerate at 200m/s/s, when the limit is 9 + EXPECT_NEAR(-9.55, result, 1e-3); + result = dynamics_limits.Limit(5, -5, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(4.55, result, 1e-3); + result = dynamics_limits.Limit(-3, -2, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(-2.55, result, 1e-3); +} + +TEST(DynamicsLimitsTest, test_Saturate) { + auto dynamics_limits = DynamicsLimits(); + double result; + // Useless, strict bounds + result = DynamicsLimits::Saturate(0, 0, 0); // Bound between 0 and 0, 0 + EXPECT_NEAR(0, result, 1e-3); + result = DynamicsLimits::Saturate(1, 0, 0); // Bound between 1 and 0, 0 + EXPECT_NEAR(0, result, 1e-3); + result = DynamicsLimits::Saturate(-1, 0, 0); // Bound between -1 and 0, 0 + EXPECT_NEAR(0, result, 1e-3); + + // Valid bounds + result = DynamicsLimits::Saturate(0, -1.1, 2.2); + EXPECT_NEAR(0, result, 1e-3); + result = DynamicsLimits::Saturate(1, -1.1, 2.2); + EXPECT_NEAR(1, result, 1e-3); + result = DynamicsLimits::Saturate(-1, -1.1, 2.2); + EXPECT_NEAR(-1, result, 1e-3); + result = DynamicsLimits::Saturate(3, -1.1, 2.2); + EXPECT_NEAR(2.2, result, 1e-3); + result = DynamicsLimits::Saturate(-3, -1.1, 2.2); + EXPECT_NEAR(-1.1, result, 1e-3); + + // Invalid bounds, input not changed + result = DynamicsLimits::Saturate(0, 1.1, -2.2); + EXPECT_NEAR(0, result, 1e-3); + result = DynamicsLimits::Saturate(1, 1.1, -2.2); + EXPECT_NEAR(1, result, 1e-3); + result = DynamicsLimits::Saturate(-1, 1.1, -2.2); + EXPECT_NEAR(-1, result, 1e-3); + result = DynamicsLimits::Saturate(3, 1.1, -2.2); + EXPECT_NEAR(3, result, 1e-3); + result = DynamicsLimits::Saturate(-3, 1.1, -2.2); + EXPECT_NEAR(-3, result, 1e-3); +} + +/** + * Test Limit function wrt. acceleration and decelleration + */ +TEST(DynamicsLimitsTest, test_Limit_deceleration) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 1.0; + config["deceleration_limit"] = 9.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 1 (acceleration) + EXPECT_NEAR(0.05, result, 1e-3); + result = dynamics_limits.Limit(0.97, 1.0, 0.05); // Try to accelerate at 1m/s/s, when the limit is 1 + EXPECT_NEAR(1.0, result, 1e-3); + result = dynamics_limits.Limit(1.0, 0.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(0.55, result, 1e-3); + result = dynamics_limits.Limit(-1.0, -2.0, 0.05); // Try to acccelerate at 20m/s/s, when the limit is 1 + EXPECT_NEAR(-1.05, result, 1e-3); + result = dynamics_limits.Limit(-2.0, -1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(-1.55, result, 1e-3); + result = dynamics_limits.Limit(-2.0, 1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(-1.55, result, 1e-3); + result = dynamics_limits.Limit(-1.0, 2.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(-0.55, result, 1e-3); + result = dynamics_limits.Limit(1.0, 3.0, 0.05); // Try to acccelerate at 20m/s/s, when the limit is 1 + EXPECT_NEAR(1.05, result, 1e-3); + result = dynamics_limits.Limit(3.0, -1.0, 0.05); // Try to decelerate at 20m/s/s, when the limit is 9 + EXPECT_NEAR(2.55, result, 1e-3); +} + +/** + * Test Limit function wrt. acceleration and decelleration + */ +TEST(DynamicsLimitsTest, test_Limit_zero_crossing) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 1.0; + config["deceleration_limit"] = 2.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(2.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep + EXPECT_NEAR(1.9, result, 1e-3); + + result = dynamics_limits.Limit(0.08, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep + // the deceletion from 0.08 -> 0m/s is done at 2m/s/s, taking 0.04s, 80% of the timestep + // then the acceleration from 0 -> -1 has the accleration constraints applied given the remainder of the timestep (0.01s) + // resulting in an effective acceleration limit of 1.8m/s/s over the timestep + EXPECT_NEAR(-0.01, result, 1e-3); +} + + +/** + * Test Limit function wrt. acceleration and decelleration corner case around zero crossing + */ +TEST(DynamicsLimitsTest, test_Limit_zero_crossing_corner_case1) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 0.0; + config["deceleration_limit"] = 2.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(0.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(2.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep + EXPECT_NEAR(1.9, result, 1e-3); + + result = dynamics_limits.Limit(0.08, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep + // The first 80% decelerates at 2m/s/s, then the remaining 20% of the timestep accelerates from 0 to -1m/s without acceleration limit + EXPECT_NEAR(-1.0, result, 1e-3); +} + +/** + * Test Limit function wrt. acceleration and decelleration corner case around zero crossing + */ +TEST(DynamicsLimitsTest, test_Limit_zero_crossing_corner_case2) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 2.0; + config["deceleration_limit"] = 0.0; + dynamics_limits.Configure(config); + + EXPECT_NEAR(2.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.0, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(2.0, 0.0, 0.05); // Decelerate, without zero crossing inside the timestep + EXPECT_NEAR(0.0, result, 1e-3); + + result = dynamics_limits.Limit(1.0, -1.0, 0.05); // Decelerate, with zero crossing inside the timestep (no limit) + // The portion from 1m/s to 0m/s has no limit, however the timestep is used to accelerate under limits + // 0.05 sec at -2m/s/s acceleration = -0.1m/s final velocity + EXPECT_NEAR(-0.1, result, 1e-3); +} + +/** + * Test Limit function wrt. acceleration asnd decelleration + */ +TEST(DynamicsLimitsTest, test_Limit_acceleration_and_velocity) { + auto dynamics_limits = DynamicsLimits(); + + YAML::Node config = YAML::Node(); + config["acceleration_limit"] = 1.0; + config["deceleration_limit"] = 9.0; + config["velocity_limit"] = 0.5; + dynamics_limits.Configure(config); + + EXPECT_NEAR(1.0, dynamics_limits.acceleration_limit_, 1e-3); + EXPECT_NEAR(9.0, dynamics_limits.deceleration_limit_, 1e-3); + EXPECT_NEAR(0.5, dynamics_limits.velocity_limit_, 1e-3); + + double result; + result = dynamics_limits.Limit(0, 1.0, 0.05); // Try to accelerate at 20m/s/s, when the limit is 1 (acceleration) + EXPECT_NEAR(0.05, result, 1e-3); + + result = dynamics_limits.Limit(0.48, 1.48, 0.05); // Same, but hit the velocity limit + EXPECT_NEAR(0.5, result, 1e-3); + + result = dynamics_limits.Limit(1.0, 2.0, 0.05); // Same, but we're already over the velocity limit + // The invalid input temporarily results in invalid output, because we have to voilate either decelleration or velocity constraints + // In this case, we violate velocity constraints + EXPECT_NEAR(0.55, result, 1e-3); + + result = dynamics_limits.Limit(0.55, 2.0, 0.05); // Same, but we're already over the velocity limit, + // but this time we can achieve velocity constraints without violating decelleration constraints + EXPECT_NEAR(0.5, result, 1e-3); +} + + + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/gps_test.cpp b/flatland_plugins/test/gps_test.cpp index b9a80d43..f26080eb 100644 --- a/flatland_plugins/test/gps_test.cpp +++ b/flatland_plugins/test/gps_test.cpp @@ -1,24 +1,24 @@ -#include -#include -#include -#include -#include - -TEST(GpsPluginTest, load_test) { - pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); - - try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::Gps"); - } catch (pluginlib::PluginlibException& e) { - FAIL() << "Failed to load GPS plugin. " << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "gps_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +#include +#include +#include +#include +#include + +TEST(GpsPluginTest, load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::Gps"); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load GPS plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "gps_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/imu_test.cpp b/flatland_plugins/test/imu_test.cpp new file mode 100644 index 00000000..c4004c72 --- /dev/null +++ b/flatland_plugins/test/imu_test.cpp @@ -0,0 +1,70 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name imu_test.cpp + * @brief test diff drive plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +TEST(ImuPluginTest, load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::Imu"); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load imu plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "imu_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/imu_test.test b/flatland_plugins/test/imu_test.test new file mode 100644 index 00000000..c3fc942f --- /dev/null +++ b/flatland_plugins/test/imu_test.test @@ -0,0 +1,9 @@ + + + + diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index ba687266..2e71b593 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -1,313 +1,313 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name laser_test.cpp - * @brief test laser plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; -using namespace flatland_plugins; - -class LaserPluginTest : public ::testing::Test { - public: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - sensor_msgs::LaserScan scan_front, scan_center, scan_back; - World* w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - static bool fltcmp(const double& n1, const double& n2) { - if (std::isinf(n1) && std::isinf(n2)) { - return true; - } - - if (std::isnan(n1) && std::isnan(n2)) { - return true; - } - - bool ret = fabs(n1 - n2) < 1e-5; - return ret; - } - - // print content of the vector for debugging - void print_flt_vec(const std::vector& v) { - printf("{"); - for (const auto& e : v) { - printf("%f,", e); - } - printf("}"); - } - - // check the float values equals and print message for debugging - bool FloatEq(const char* name, float actual, float expected) { - if (actual != expected) { - printf("%s Actual:%f != Expected %f", name, actual, expected); - return false; - } - return true; - } - - // check the received scan data is as expected - bool ScanEq(const sensor_msgs::LaserScan& scan, std::string frame_id, - float angle_min, float angle_max, float angle_increment, - float time_increment, float scan_time, float range_min, - float range_max, std::vector ranges, - std::vector intensities) { - if (scan.header.frame_id != frame_id) { - printf("frame_id Actual:%s != Expected:%s\n", - scan.header.frame_id.c_str(), frame_id.c_str()); - return false; - } - - if (!FloatEq("angle_min", scan.angle_min, angle_min)) return false; - if (!FloatEq("angle_max", scan.angle_max, angle_max)) return false; - if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) - return false; - if (!FloatEq("time_increment", scan.time_increment, time_increment)) - return false; - if (!FloatEq("scan_time", scan.scan_time, scan_time)) return false; - if (!FloatEq("range_min", scan.range_min, range_min)) return false; - if (!FloatEq("range_max", scan.range_max, range_max)) return false; - - if (ranges.size() != scan.ranges.size() || - !std::equal(ranges.begin(), ranges.end(), scan.ranges.begin(), - fltcmp)) { - printf("\"ranges\" does not match\n"); - printf("Actual: "); - print_flt_vec(scan.ranges); - printf("\n"); - printf("Expected: "); - print_flt_vec(ranges); - printf("\n"); - return false; - } - - if (intensities.size() != scan.intensities.size() || - !std::equal(intensities.begin(), intensities.end(), - scan.intensities.begin(), fltcmp)) { - printf("\"intensities\" does not math"); - printf("Actual: "); - print_flt_vec(scan.intensities); - printf("\n"); - printf("Expected: "); - print_flt_vec(intensities); - printf("\n"); - return false; - } - - return true; - } - - void ScanFrontCb(const sensor_msgs::LaserScan& msg) { scan_front = msg; }; - void ScanCenterCb(const sensor_msgs::LaserScan& msg) { scan_center = msg; }; - void ScanBackCb(const sensor_msgs::LaserScan& msg) { scan_back = msg; }; -}; - -/** - * Test the laser plugin for a given model and plugin configuration - */ -TEST_F(LaserPluginTest, range_test) { - world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); - - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; - LaserPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); - sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); - sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); - - Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); - - // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); - for (unsigned int i = 0; i < 10; i++) { - w->Update(timekeeper); - ros::spinOnce(); - rate.sleep(); - } - - // check scan returns - EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, - 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {})); - EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) - << "Actual: " << p1->update_rate_; - EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); - - EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, - 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, {})); - EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; - EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); - - EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, - 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {})); - EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; - EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); -} -/** - * Test the laser plugin for intensity configuration - */ -TEST_F(LaserPluginTest, intensity_test) { - world_yaml = - this_file_dir / fs::path("laser_tests/intensity_test/world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); - - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; - LaserPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); - sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); - sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); - - Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); - - // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); - for (unsigned int i = 0; i < 10; i++) { - w->Update(timekeeper); - ros::spinOnce(); - rate.sleep(); - } - - // check scan returns - EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, - 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {0, 0, 0})); - EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) - << "Actual: " << p1->update_rate_; - EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, - 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, - {0, 255, 0, 0, 0})); - EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; - EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, - 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {0, 0, 0, 0, 0})); - EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; - EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); -} - -/** - * Checks the laser plugin will throw correct exception for invalid - * configurations - */ -TEST_F(LaserPluginTest, invalid_A) { - world_yaml = this_file_dir / fs::path("laser_tests/invalid_A/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { - std::cmatch match; - std::string regex_str = ".*Flatland YAML: Entry \"range\" does not exist.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -/** - * Checks the laser plugin will throw correct exception for invalid - * configurations - */ -TEST_F(LaserPluginTest, invalid_B) { - world_yaml = this_file_dir / fs::path("laser_tests/invalid_B/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { - std::cmatch match; - std::string regex_str = ".*Invalid \"angle\" params, must have max > min.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "laser_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name laser_test.cpp + * @brief test laser plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; +using namespace flatland_plugins; + +class LaserPluginTest : public ::testing::Test { + public: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + sensor_msgs::LaserScan scan_front, scan_center, scan_back; + World* w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + static bool fltcmp(const double& n1, const double& n2) { + if (std::isinf(n1) && std::isinf(n2)) { + return true; + } + + if (std::isnan(n1) && std::isnan(n2)) { + return true; + } + + bool ret = fabs(n1 - n2) < 1e-5; + return ret; + } + + // print content of the vector for debugging + void print_flt_vec(const std::vector& v) { + printf("{"); + for (const auto& e : v) { + printf("%f,", e); + } + printf("}"); + } + + // check the float values equals and print message for debugging + bool FloatEq(const char* name, float actual, float expected) { + if (actual != expected) { + printf("%s Actual:%f != Expected %f", name, actual, expected); + return false; + } + return true; + } + + // check the received scan data is as expected + bool ScanEq(const sensor_msgs::LaserScan& scan, std::string frame_id, + float angle_min, float angle_max, float angle_increment, + float time_increment, float scan_time, float range_min, + float range_max, std::vector ranges, + std::vector intensities) { + if (scan.header.frame_id != frame_id) { + printf("frame_id Actual:%s != Expected:%s\n", + scan.header.frame_id.c_str(), frame_id.c_str()); + return false; + } + + if (!FloatEq("angle_min", scan.angle_min, angle_min)) return false; + if (!FloatEq("angle_max", scan.angle_max, angle_max)) return false; + if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) + return false; + if (!FloatEq("time_increment", scan.time_increment, time_increment)) + return false; + if (!FloatEq("scan_time", scan.scan_time, scan_time)) return false; + if (!FloatEq("range_min", scan.range_min, range_min)) return false; + if (!FloatEq("range_max", scan.range_max, range_max)) return false; + + if (ranges.size() != scan.ranges.size() || + !std::equal(ranges.begin(), ranges.end(), scan.ranges.begin(), + fltcmp)) { + printf("\"ranges\" does not match\n"); + printf("Actual: "); + print_flt_vec(scan.ranges); + printf("\n"); + printf("Expected: "); + print_flt_vec(ranges); + printf("\n"); + return false; + } + + if (intensities.size() != scan.intensities.size() || + !std::equal(intensities.begin(), intensities.end(), + scan.intensities.begin(), fltcmp)) { + printf("\"intensities\" does not math"); + printf("Actual: "); + print_flt_vec(scan.intensities); + printf("\n"); + printf("Expected: "); + print_flt_vec(intensities); + printf("\n"); + return false; + } + + return true; + } + + void ScanFrontCb(const sensor_msgs::LaserScan& msg) { scan_front = msg; }; + void ScanCenterCb(const sensor_msgs::LaserScan& msg) { scan_center = msg; }; + void ScanBackCb(const sensor_msgs::LaserScan& msg) { scan_back = msg; }; +}; + +/** + * Test the laser plugin for a given model and plugin configuration + */ +TEST_F(LaserPluginTest, range_test) { + world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + + ros::NodeHandle nh; + ros::Subscriber sub_1, sub_2, sub_3; + LaserPluginTest* obj = dynamic_cast(this); + sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); + sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); + sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); + + Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned int i = 0; i < 10; i++) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + // check scan returns + EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, + 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {})); + EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) + << "Actual: " << p1->update_rate_; + EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); + + EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, + 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, {})); + EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; + EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); + + EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, + 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {})); + EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; + EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); +} +/** + * Test the laser plugin for intensity configuration + */ +TEST_F(LaserPluginTest, intensity_test) { + world_yaml = + this_file_dir / fs::path("laser_tests/intensity_test/world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + + ros::NodeHandle nh; + ros::Subscriber sub_1, sub_2, sub_3; + LaserPluginTest* obj = dynamic_cast(this); + sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); + sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); + sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); + + Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned int i = 0; i < 10; i++) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + // check scan returns + EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, + 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {0, 0, 0})); + EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) + << "Actual: " << p1->update_rate_; + EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); + EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, + 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, + {0, 255, 0, 0, 0})); + EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; + EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); + EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, + 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {0, 0, 0, 0, 0})); + EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; + EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); +} + +/** + * Checks the laser plugin will throw correct exception for invalid + * configurations + */ +TEST_F(LaserPluginTest, invalid_A) { + world_yaml = this_file_dir / fs::path("laser_tests/invalid_A/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException& e) { + std::cmatch match; + std::string regex_str = ".*Flatland YAML: Entry \"range\" does not exist.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception& e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +/** + * Checks the laser plugin will throw correct exception for invalid + * configurations + */ +TEST_F(LaserPluginTest, invalid_B) { + world_yaml = this_file_dir / fs::path("laser_tests/invalid_B/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException& e) { + std::cmatch match; + std::string regex_str = ".*Invalid \"angle\" params, must have max > min.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception& e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "laser_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/laser_tests/intensity_test/world.yaml b/flatland_plugins/test/laser_tests/intensity_test/world.yaml index 61a7e67a..acd4fdf1 100644 --- a/flatland_plugins/test/laser_tests/intensity_test/world.yaml +++ b/flatland_plugins/test/laser_tests/intensity_test/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/laser_tests/intensity_test/world_plugins.yaml b/flatland_plugins/test/laser_tests/intensity_test/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/laser_tests/intensity_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/laser_tests/invalid_A/world.yaml b/flatland_plugins/test/laser_tests/invalid_A/world.yaml index 3089ab15..6b1a2ce2 100644 --- a/flatland_plugins/test/laser_tests/invalid_A/world.yaml +++ b/flatland_plugins/test/laser_tests/invalid_A/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../range_test/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/laser_tests/invalid_A/world_plugins.yaml b/flatland_plugins/test/laser_tests/invalid_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/laser_tests/invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/laser_tests/invalid_B/world.yaml b/flatland_plugins/test/laser_tests/invalid_B/world.yaml index 3089ab15..6b1a2ce2 100644 --- a/flatland_plugins/test/laser_tests/invalid_B/world.yaml +++ b/flatland_plugins/test/laser_tests/invalid_B/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../range_test/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/laser_tests/invalid_B/world_plugins.yaml b/flatland_plugins/test/laser_tests/invalid_B/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/laser_tests/invalid_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml new file mode 100644 index 00000000..5c634bb0 --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.asymmetrical.yaml @@ -0,0 +1,34 @@ +# Turtlebot + +bodies: # List of named bodies + - name: base_link + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 0, 1] + footprints: + - type: polygon + density: 1.0 + points: [[-.5, -0.25], [-.5, 0.25], [.5, 0.25], [.5, -0.25]] + +plugins: + - type: Laser + name: laser_flipped + body: base_link + range: 5 + topic: scan1 + frame: laser_flipped_custom + upside_down: true + angle: {min: 0, max: 0.21, increment: 0.1} + - type: Laser + name: laser_normal + body: base_link + range: 5 + topic: scan2 + upside_down: false + angle: {min: 0, max: 0.21, increment: 0.1} + - type: ModelTfPublisher + name: state_publisher + update_rate: .inf + publish_tf_world: true + world_frame_id: map + reference: base_link \ No newline at end of file diff --git a/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml new file mode 100644 index 00000000..318f8df1 --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/robot.model.cw.yaml @@ -0,0 +1,20 @@ +# Turtlebot + +bodies: # List of named bodies + - name: base_link + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 0, 1] + footprints: + - type: circle + density: 1 + center: [0, 0] + radius: 0.1 + +plugins: + - type: Laser + name: laser_front + body: base_link + range: 5 + upside_down: true + angle: {min: -1.5707963267948966, max: 1.5707963267948966, increment: 1.5707963267948966} diff --git a/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml b/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml new file mode 100644 index 00000000..9a20cd8a --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/world.cw.asymmetrical.yaml @@ -0,0 +1,13 @@ +properties: {} +layers: + - name: "layer_1" + map: "map_1.yaml" + color: [0, 1, 0, 1] + - name: "layer_2" + map: "map_2.yaml" + color: [0, 1, 0, 1] +models: + - name: robot1 + pose: [5, 5, 0.2] + model: robot.model.cw.asymmetrical.yaml + namespace: "r" diff --git a/flatland_plugins/test/laser_tests/range_test/world.cw.yaml b/flatland_plugins/test/laser_tests/range_test/world.cw.yaml new file mode 100644 index 00000000..44738ffa --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/world.cw.yaml @@ -0,0 +1,13 @@ +properties: {} +layers: + - name: "layer_1" + map: "map_1.yaml" + color: [0, 1, 0, 1] + - name: "layer_2" + map: "map_2.yaml" + color: [0, 1, 0, 1] +models: + - name: robot1 + pose: [5, 5, 0] + model: robot.model.cw.yaml + namespace: "r" diff --git a/flatland_plugins/test/laser_tests/range_test/world.yaml b/flatland_plugins/test/laser_tests/range_test/world.yaml index 19ca46d6..8cc50251 100644 --- a/flatland_plugins/test/laser_tests/range_test/world.yaml +++ b/flatland_plugins/test/laser_tests/range_test/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/laser_tests/range_test/world_plugins.yaml b/flatland_plugins/test/laser_tests/range_test/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/laser_tests/range_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index 19a695ce..c304b251 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -1,312 +1,312 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_tf_publisher_test.cpp - * @brief test model TF publisher plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; -using namespace flatland_plugins; - -class ModelTfPublisherTest : public ::testing::Test { - public: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - World* w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - static bool fltcmp(const double& n1, const double& n2) { - if (std::isinf(n1) && std::isinf(n2)) { - return true; - } - - if (std::isnan(n1) && std::isnan(n2)) { - return true; - } - - bool ret = fabs(n1 - n2) < 1e-5; - return ret; - } - - // Test if transform equals to expected - bool TfEq(const geometry_msgs::TransformStamped& tf, float x, float y, - float a) { - tf::Quaternion q; - tf::quaternionMsgToTF(tf.transform.rotation, q); - tf::Matrix3x3 rot_matrix(q); - double roll, pitch, yaw; - rot_matrix.getRPY(roll, pitch, yaw); - - if (!fltcmp(x, tf.transform.translation.x) || - !fltcmp(y, tf.transform.translation.y) || - !fltcmp(0, tf.transform.translation.z) || !fltcmp(roll, 0) || - !fltcmp(pitch, 0) || !fltcmp(yaw, a)) { - printf("Transformation\n"); - printf("Actual: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", - tf.transform.translation.x, tf.transform.translation.y, - tf.transform.translation.z, roll, pitch, yaw); - printf("Expected: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", x, y, 0.0, - 0.0, 0.0, a); - return false; - } - - return true; - } -}; - -/** - * Test the transformation for the model robot in a given plugin configuration - */ -TEST_F(ModelTfPublisherTest, tf_publish_test_A) { - world_yaml = - this_file_dir / - fs::path("model_tf_publisher_tests/tf_publish_test_A/world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); - ModelTfPublisher* p = dynamic_cast( - w->plugin_manager_.model_plugins_[0].get()); - - EXPECT_DOUBLE_EQ(5000.0, p->update_rate_); - EXPECT_STREQ("antenna", p->reference_body_->name_.c_str()); - - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - geometry_msgs::TransformStamped tf_world_to_base; - geometry_msgs::TransformStamped tf_world_to_antenna; - geometry_msgs::TransformStamped tf_base_to_left_wheel; - geometry_msgs::TransformStamped tf_base_to_right_wheel; - geometry_msgs::TransformStamped tf_base_to_front_bumper; - geometry_msgs::TransformStamped tf_base_to_rear_bumper; - - // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); - for (unsigned int i = 0; i < 100; i++) { - w->Update(timekeeper); - ros::spinOnce(); - rate.sleep(); - } - - // check for the transformations that should exist - tf_world_to_base = - tf_buffer.lookupTransform("world", "my_robot_base", ros::Time(0)); - tf_world_to_antenna = - tf_buffer.lookupTransform("world", "my_robot_antenna", ros::Time(0)); - tf_base_to_left_wheel = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_left_wheel", ros::Time(0)); - tf_base_to_right_wheel = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_right_wheel", ros::Time(0)); - - // check for the transformations that should not exist - try { - tf_base_to_front_bumper = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_front_bumper", ros::Time(0)); - ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { - EXPECT_STREQ( - "\"my_robot_front_bumper\" passed to lookupTransform argument " - "source_frame does not exist. ", - e.what()); - } - - try { - tf_base_to_rear_bumper = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_rear_bumper", ros::Time(0)); - ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { - EXPECT_STREQ( - "\"my_robot_rear_bumper\" passed to lookupTransform argument " - "source_frame does not exist. ", - e.what()); - } - - // check transformations are correct - EXPECT_TRUE(TfEq(tf_world_to_base, 8, 6, -0.575958653)); - EXPECT_TRUE(TfEq(tf_world_to_antenna, 8, 6, -0.575958653)); - EXPECT_TRUE(TfEq(tf_base_to_left_wheel, -0.25, 1, 0)); - EXPECT_TRUE(TfEq(tf_base_to_right_wheel, -0.25, -1, 0)); -} - -/** - * Test the transformation for the model robot in another plugin configuration - */ -TEST_F(ModelTfPublisherTest, tf_publish_test_B) { - world_yaml = - this_file_dir / - fs::path("model_tf_publisher_tests/tf_publish_test_B/world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); - ModelTfPublisher* p = dynamic_cast( - w->plugin_manager_.model_plugins_[0].get()); - - EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), p->update_rate_); - EXPECT_STREQ("base", p->reference_body_->name_.c_str()); - - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); - geometry_msgs::TransformStamped tf_map_to_base; - geometry_msgs::TransformStamped tf_base_to_antenna; - geometry_msgs::TransformStamped tf_base_to_left_wheel; - geometry_msgs::TransformStamped tf_base_to_right_wheel; - geometry_msgs::TransformStamped tf_base_to_front_bumper; - geometry_msgs::TransformStamped tf_base_to_rear_bumper; - - // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); - for (unsigned int i = 0; i < 100; i++) { - w->Update(timekeeper); - ros::spinOnce(); - rate.sleep(); - } - - tf_base_to_antenna = - tf_buffer.lookupTransform("base", "antenna", ros::Time(0)); - tf_base_to_left_wheel = - tf_buffer.lookupTransform("base", "left_wheel", ros::Time(0)); - tf_base_to_right_wheel = - tf_buffer.lookupTransform("base", "right_wheel", ros::Time(0)); - tf_base_to_front_bumper = - tf_buffer.lookupTransform("base", "front_bumper", ros::Time(0)); - tf_base_to_rear_bumper = - tf_buffer.lookupTransform("base", "rear_bumper", ros::Time(0)); - - try { - tf_map_to_base = tf_buffer.lookupTransform("map", "base", ros::Time(0)); - ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { - EXPECT_STREQ( - "\"map\" passed to lookupTransform argument target_frame does not " - "exist. ", - e.what()); - } - - EXPECT_TRUE(TfEq(tf_base_to_antenna, 0, 0, 0)); - EXPECT_TRUE(TfEq(tf_base_to_left_wheel, -0.25, 1, 0)); - EXPECT_TRUE(TfEq(tf_base_to_right_wheel, -0.25, -1, 0)); - EXPECT_TRUE(TfEq(tf_base_to_front_bumper, 2, 0, 0)); - EXPECT_TRUE(TfEq(tf_base_to_rear_bumper, -2, 0, 0)); -} - -/** - * Test the transformation for the provided model yaml, which will fail due - * to a nonexistent reference body - */ -TEST_F(ModelTfPublisherTest, invalid_A) { - world_yaml = - this_file_dir / fs::path("model_tf_publisher_tests/invalid_A/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { - std::cmatch match; - std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -/** - * Test the transformation for the provided model yaml, which will fail due - * to a nonexistent body specified in the exclude list - */ -TEST_F(ModelTfPublisherTest, invalid_B) { - world_yaml = - this_file_dir / fs::path("model_tf_publisher_tests/invalid_B/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { - std::cmatch match; - std::string regex_str = - ".*Body with name \"random_body_1\" does not exist.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "model_tf_plugin_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_tf_publisher_test.cpp + * @brief test model TF publisher plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; +using namespace flatland_plugins; + +class ModelTfPublisherTest : public ::testing::Test { + public: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + World* w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + static bool fltcmp(const double& n1, const double& n2) { + if (std::isinf(n1) && std::isinf(n2)) { + return true; + } + + if (std::isnan(n1) && std::isnan(n2)) { + return true; + } + + bool ret = fabs(n1 - n2) < 1e-5; + return ret; + } + + // Test if transform equals to expected + bool TfEq(const geometry_msgs::TransformStamped& tf, float x, float y, + float a) { + tf::Quaternion q; + tf::quaternionMsgToTF(tf.transform.rotation, q); + tf::Matrix3x3 rot_matrix(q); + double roll, pitch, yaw; + rot_matrix.getRPY(roll, pitch, yaw); + + if (!fltcmp(x, tf.transform.translation.x) || + !fltcmp(y, tf.transform.translation.y) || + !fltcmp(0, tf.transform.translation.z) || !fltcmp(roll, 0) || + !fltcmp(pitch, 0) || !fltcmp(yaw, a)) { + printf("Transformation\n"); + printf("Actual: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", + tf.transform.translation.x, tf.transform.translation.y, + tf.transform.translation.z, roll, pitch, yaw); + printf("Expected: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", x, y, 0.0, + 0.0, 0.0, a); + return false; + } + + return true; + } +}; + +/** + * Test the transformation for the model robot in a given plugin configuration + */ +TEST_F(ModelTfPublisherTest, tf_publish_test_A) { + world_yaml = + this_file_dir / + fs::path("model_tf_publisher_tests/tf_publish_test_A/world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + ModelTfPublisher* p = dynamic_cast( + w->plugin_manager_.model_plugins_[0].get()); + + EXPECT_DOUBLE_EQ(5000.0, p->update_rate_); + EXPECT_STREQ("antenna", p->reference_body_->name_.c_str()); + + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener(tf_buffer); + geometry_msgs::TransformStamped tf_world_to_base; + geometry_msgs::TransformStamped tf_world_to_antenna; + geometry_msgs::TransformStamped tf_base_to_left_wheel; + geometry_msgs::TransformStamped tf_base_to_right_wheel; + geometry_msgs::TransformStamped tf_base_to_front_bumper; + geometry_msgs::TransformStamped tf_base_to_rear_bumper; + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned int i = 0; i < 100; i++) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + // check for the transformations that should exist + tf_world_to_base = + tf_buffer.lookupTransform("world", "my_robot_base", ros::Time(0)); + tf_world_to_antenna = + tf_buffer.lookupTransform("world", "my_robot_antenna", ros::Time(0)); + tf_base_to_left_wheel = tf_buffer.lookupTransform( + "my_robot_base", "my_robot_left_wheel", ros::Time(0)); + tf_base_to_right_wheel = tf_buffer.lookupTransform( + "my_robot_base", "my_robot_right_wheel", ros::Time(0)); + + // check for the transformations that should not exist + try { + tf_base_to_front_bumper = tf_buffer.lookupTransform( + "my_robot_base", "my_robot_front_bumper", ros::Time(0)); + ADD_FAILURE() << "Expected an exception, but none were raised"; + } catch (const tf2::TransformException& e) { + EXPECT_STREQ( + "\"my_robot_front_bumper\" passed to lookupTransform argument " + "source_frame does not exist. ", + e.what()); + } + + try { + tf_base_to_rear_bumper = tf_buffer.lookupTransform( + "my_robot_base", "my_robot_rear_bumper", ros::Time(0)); + ADD_FAILURE() << "Expected an exception, but none were raised"; + } catch (const tf2::TransformException& e) { + EXPECT_STREQ( + "\"my_robot_rear_bumper\" passed to lookupTransform argument " + "source_frame does not exist. ", + e.what()); + } + + // check transformations are correct + EXPECT_TRUE(TfEq(tf_world_to_base, 8, 6, -0.575958653)); + EXPECT_TRUE(TfEq(tf_world_to_antenna, 8, 6, -0.575958653)); + EXPECT_TRUE(TfEq(tf_base_to_left_wheel, -0.25, 1, 0)); + EXPECT_TRUE(TfEq(tf_base_to_right_wheel, -0.25, -1, 0)); +} + +/** + * Test the transformation for the model robot in another plugin configuration + */ +TEST_F(ModelTfPublisherTest, tf_publish_test_B) { + world_yaml = + this_file_dir / + fs::path("model_tf_publisher_tests/tf_publish_test_B/world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + ModelTfPublisher* p = dynamic_cast( + w->plugin_manager_.model_plugins_[0].get()); + + EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), p->update_rate_); + EXPECT_STREQ("base", p->reference_body_->name_.c_str()); + + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener(tf_buffer); + geometry_msgs::TransformStamped tf_map_to_base; + geometry_msgs::TransformStamped tf_base_to_antenna; + geometry_msgs::TransformStamped tf_base_to_left_wheel; + geometry_msgs::TransformStamped tf_base_to_right_wheel; + geometry_msgs::TransformStamped tf_base_to_front_bumper; + geometry_msgs::TransformStamped tf_base_to_rear_bumper; + + // let it spin for 10 times to make sure the message gets through + ros::WallRate rate(500); + for (unsigned int i = 0; i < 100; i++) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + tf_base_to_antenna = + tf_buffer.lookupTransform("base", "antenna", ros::Time(0)); + tf_base_to_left_wheel = + tf_buffer.lookupTransform("base", "left_wheel", ros::Time(0)); + tf_base_to_right_wheel = + tf_buffer.lookupTransform("base", "right_wheel", ros::Time(0)); + tf_base_to_front_bumper = + tf_buffer.lookupTransform("base", "front_bumper", ros::Time(0)); + tf_base_to_rear_bumper = + tf_buffer.lookupTransform("base", "rear_bumper", ros::Time(0)); + + try { + tf_map_to_base = tf_buffer.lookupTransform("map", "base", ros::Time(0)); + ADD_FAILURE() << "Expected an exception, but none were raised"; + } catch (const tf2::TransformException& e) { + EXPECT_STREQ( + "\"map\" passed to lookupTransform argument target_frame does not " + "exist. ", + e.what()); + } + + EXPECT_TRUE(TfEq(tf_base_to_antenna, 0, 0, 0)); + EXPECT_TRUE(TfEq(tf_base_to_left_wheel, -0.25, 1, 0)); + EXPECT_TRUE(TfEq(tf_base_to_right_wheel, -0.25, -1, 0)); + EXPECT_TRUE(TfEq(tf_base_to_front_bumper, 2, 0, 0)); + EXPECT_TRUE(TfEq(tf_base_to_rear_bumper, -2, 0, 0)); +} + +/** + * Test the transformation for the provided model yaml, which will fail due + * to a nonexistent reference body + */ +TEST_F(ModelTfPublisherTest, invalid_A) { + world_yaml = + this_file_dir / fs::path("model_tf_publisher_tests/invalid_A/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException& e) { + std::cmatch match; + std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception& e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +/** + * Test the transformation for the provided model yaml, which will fail due + * to a nonexistent body specified in the exclude list + */ +TEST_F(ModelTfPublisherTest, invalid_B) { + world_yaml = + this_file_dir / fs::path("model_tf_publisher_tests/invalid_B/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException& e) { + std::cmatch match; + std::string regex_str = + ".*Body with name \"random_body_1\" does not exist.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception& e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "model_tf_plugin_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world.yaml b/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world.yaml index 0478fc79..0f8d09d0 100644 --- a/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world.yaml +++ b/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../tf_publish_test_A/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world_plugins.yaml b/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/model_tf_publisher_tests/invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world.yaml b/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world.yaml index 0478fc79..0f8d09d0 100644 --- a/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world.yaml +++ b/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../tf_publish_test_A/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world_plugins.yaml b/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/model_tf_publisher_tests/invalid_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world.yaml b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world.yaml index be1d9b19..ca0799eb 100644 --- a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world.yaml +++ b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world_plugins.yaml b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world.yaml b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world.yaml index 7681e29a..05638630 100644 --- a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world.yaml +++ b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "../tf_publish_test_A/map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world_plugins.yaml b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/model_tf_publisher_tests/tf_publish_test_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/tricycle_drive_test.cpp b/flatland_plugins/test/tricycle_drive_test.cpp index 5d4b5fa2..b781b72a 100644 --- a/flatland_plugins/test/tricycle_drive_test.cpp +++ b/flatland_plugins/test/tricycle_drive_test.cpp @@ -1,70 +1,70 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name tricycle_drive_test.cpp - * @brief test tricycle drive plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -TEST(TricycleDrivePluginTest, load_test) { - pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); - - try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::TricycleDrive"); - } catch (pluginlib::PluginlibException& e) { - FAIL() << "Failed to load Tricycle Drive plugin. " << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "tricycle_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name tricycle_drive_test.cpp + * @brief test tricycle drive plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +TEST(TricycleDrivePluginTest, load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::TricycleDrive"); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load Tricycle Drive plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "tricycle_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/tricycle_drive_tests/cleaner.model.yaml b/flatland_plugins/test/tricycle_drive_tests/cleaner.model.yaml new file mode 100644 index 00000000..0c3f9961 --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/cleaner.model.yaml @@ -0,0 +1,93 @@ + +bodies: + - name: base + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 100 + points: [ [-1.03, -0.337], + [.07983, -0.337], + [.30, -.16111], + [.30, .16111], + [.07983, 0.337], + [-1.03, 0.337] ] + - name: front_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0250], + [ 0.0875, 0.0250], + [-0.0875, 0.0250], + [-0.0875, -0.0250]] + + - name: rear_left_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + + - name: rear_right_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + +joints: + - type: revolute + name: front_wheel_revolute + bodies: + - name: front_wheel + anchor: [0, 0] + - name: base + anchor: [0, 0] + + - type: weld + name: rear_right_wheel_weld + bodies: + - name: rear_left_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, 0.29] + + - type: weld + name: rear_left_wheel_weld + bodies: + - name: rear_right_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, -0.29] + +plugins: + - type: TricycleDrive + name: cleaner_drive + body: base + front_wheel_joint: front_wheel_revolute + rear_left_wheel_joint: rear_left_wheel_weld + rear_right_wheel_joint: rear_right_wheel_weld + odom_frame_id: map + + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: Laser + name: laser_front + frame: laser_front + topic: scan + body: base + broadcast_tf: true + origin: [0.28, 0, 0] + range: 20 + angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} + noise_std_dev: 0.05 + update_rate: 40 diff --git a/flatland_plugins/test/tricycle_drive_tests/cleaner2.model.yaml b/flatland_plugins/test/tricycle_drive_tests/cleaner2.model.yaml new file mode 100644 index 00000000..aacc210b --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/cleaner2.model.yaml @@ -0,0 +1,95 @@ + +bodies: + - name: base + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 100 + points: [ [-1.03, -0.337], + [.07983, -0.337], + [.30, -.16111], + [.30, .16111], + [.07983, 0.337], + [-1.03, 0.337] ] + - name: front_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0250], + [ 0.0875, 0.0250], + [-0.0875, 0.0250], + [-0.0875, -0.0250]] + + - name: rear_left_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + + - name: rear_right_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + +joints: + - type: revolute + name: front_wheel_revolute + bodies: + - name: front_wheel + anchor: [0, 0] + - name: base + anchor: [0, 0] + + - type: weld + name: rear_right_wheel_weld + bodies: + - name: rear_left_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, 0.29] + + - type: weld + name: rear_left_wheel_weld + bodies: + - name: rear_right_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, -0.29] + +plugins: + - type: TricycleDrive + name: cleaner_drive + body: base + front_wheel_joint: front_wheel_revolute + rear_left_wheel_joint: rear_left_wheel_weld + rear_right_wheel_joint: rear_right_wheel_weld + odom_frame_id: map + linear_dynamics: + velocity_limit: 0.2 + + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: Laser + name: laser_front + frame: laser_front + topic: scan + body: base + broadcast_tf: true + origin: [0.28, 0, 0] + range: 20 + angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} + noise_std_dev: 0.05 + update_rate: 40 diff --git a/flatland_plugins/test/tricycle_drive_tests/cleaner3.model.yaml b/flatland_plugins/test/tricycle_drive_tests/cleaner3.model.yaml new file mode 100644 index 00000000..cc526083 --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/cleaner3.model.yaml @@ -0,0 +1,95 @@ + +bodies: + - name: base + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 100 + points: [ [-1.03, -0.337], + [.07983, -0.337], + [.30, -.16111], + [.30, .16111], + [.07983, 0.337], + [-1.03, 0.337] ] + - name: front_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0250], + [ 0.0875, 0.0250], + [-0.0875, 0.0250], + [-0.0875, -0.0250]] + + - name: rear_left_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + + - name: rear_right_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + +joints: + - type: revolute + name: front_wheel_revolute + bodies: + - name: front_wheel + anchor: [0, 0] + - name: base + anchor: [0, 0] + + - type: weld + name: rear_right_wheel_weld + bodies: + - name: rear_left_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, 0.29] + + - type: weld + name: rear_left_wheel_weld + bodies: + - name: rear_right_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, -0.29] + +plugins: + - type: TricycleDrive + name: cleaner_drive + body: base + front_wheel_joint: front_wheel_revolute + rear_left_wheel_joint: rear_left_wheel_weld + rear_right_wheel_joint: rear_right_wheel_weld + odom_frame_id: map + angular_dynamics: + velocity_limit: 0.2 + + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: Laser + name: laser_front + frame: laser_front + topic: scan + body: base + broadcast_tf: true + origin: [0.28, 0, 0] + range: 20 + angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} + noise_std_dev: 0.05 + update_rate: 40 diff --git a/flatland_plugins/test/tricycle_drive_tests/map.png b/flatland_plugins/test/tricycle_drive_tests/map.png new file mode 100644 index 00000000..a29b0ba1 Binary files /dev/null and b/flatland_plugins/test/tricycle_drive_tests/map.png differ diff --git a/flatland_plugins/test/tricycle_drive_tests/map.yaml b/flatland_plugins/test/tricycle_drive_tests/map.yaml new file mode 100644 index 00000000..ac269fbf --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/map.yaml @@ -0,0 +1,7 @@ +image: map.png +resolution: 0.050000 +origin: [-16.600000, -6.650000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/flatland_plugins/test/tricycle_drive_tests/map3d.png b/flatland_plugins/test/tricycle_drive_tests/map3d.png new file mode 100644 index 00000000..acc07667 Binary files /dev/null and b/flatland_plugins/test/tricycle_drive_tests/map3d.png differ diff --git a/flatland_plugins/test/tricycle_drive_tests/map3d.yaml b/flatland_plugins/test/tricycle_drive_tests/map3d.yaml new file mode 100644 index 00000000..6cec7384 --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/map3d.yaml @@ -0,0 +1,6 @@ +image: map3d.png +resolution: 0.050000 +origin: [-16.600000, -6.650000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 \ No newline at end of file diff --git a/flatland_plugins/test/tricycle_drive_tests/world.yaml b/flatland_plugins/test/tricycle_drive_tests/world.yaml new file mode 100644 index 00000000..0c71ee5f --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/world.yaml @@ -0,0 +1,13 @@ +properties: {} # For later use +layers: # Support for arbitrary number of layers + - name: "2d" # layer 0 named "2d" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + - name: "3d" # layer 1 named "3d" + map: "map3d.yaml" + color: [1.0, 0.0, 0.0, 0.5] # red 50% transparent +models: + - name: cleaner1 + pose: [12, 0, 0] + model: "cleaner.model.yaml" + \ No newline at end of file diff --git a/flatland_plugins/test/tricycle_drive_tests/world2.yaml b/flatland_plugins/test/tricycle_drive_tests/world2.yaml new file mode 100644 index 00000000..2cbafc12 --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/world2.yaml @@ -0,0 +1,13 @@ +properties: {} # For later use +layers: # Support for arbitrary number of layers + - name: "2d" # layer 0 named "2d" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + - name: "3d" # layer 1 named "3d" + map: "map3d.yaml" + color: [1.0, 0.0, 0.0, 0.5] # red 50% transparent +models: + - name: cleaner1 + pose: [12, 0, 0] + model: "cleaner2.model.yaml" + \ No newline at end of file diff --git a/flatland_plugins/test/tricycle_drive_tests/world3.yaml b/flatland_plugins/test/tricycle_drive_tests/world3.yaml new file mode 100644 index 00000000..ebf5dd95 --- /dev/null +++ b/flatland_plugins/test/tricycle_drive_tests/world3.yaml @@ -0,0 +1,13 @@ +properties: {} # For later use +layers: # Support for arbitrary number of layers + - name: "2d" # layer 0 named "2d" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + - name: "3d" # layer 1 named "3d" + map: "map3d.yaml" + color: [1.0, 0.0, 0.0, 0.5] # red 50% transparent +models: + - name: cleaner1 + pose: [12, 0, 0] + model: "cleaner3.model.yaml" + \ No newline at end of file diff --git a/flatland_plugins/test/tween_test.cpp b/flatland_plugins/test/tween_test.cpp index b25c1be4..757ec514 100644 --- a/flatland_plugins/test/tween_test.cpp +++ b/flatland_plugins/test/tween_test.cpp @@ -1,207 +1,207 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name tween_test.cpp - * @brief test tween test plugin - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; -using namespace flatland_plugins; - -class TweenPluginTest : public ::testing::Test { - public: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - } - - static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { - if (std::isinf(n1) && std::isinf(n2)) { - return true; - } - - if (std::isnan(n1) && std::isnan(n2)) { - return true; - } - - bool ret = fabs(n1 - n2) < epsilon; - return ret; - } -}; - -/** - * Test the tween plugin handles oneshot - */ -TEST_F(TweenPluginTest, once_test) { - world_yaml = this_file_dir / fs::path("tween_tests/once.world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); - - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - - Body* b = tween->body_; - - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 2.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 1.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 1.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 3.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 4.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 2.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 3.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 4.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 2.0)); - - delete w; -} - -/** - * Test that the tween plugin yoyos - */ -TEST_F(TweenPluginTest, yoyo_test) { - world_yaml = this_file_dir / fs::path("tween_tests/yoyo.world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); - - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - - Body* b = tween->body_; - - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.5)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 10.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 10.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 1.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.5)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.5)); - w->Update(timekeeper); - - delete w; -} - -/** - * Test that the tween plugin loops - */ -TEST_F(TweenPluginTest, loop_test) { - world_yaml = this_file_dir / fs::path("tween_tests/loop.world.yaml"); - - Timekeeper timekeeper; - timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); - - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - - Body* b = tween->body_; - - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 0.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.25)); - w->Update(timekeeper); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 7.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 7.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.75)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 10)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 10)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 1.0)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 2.5)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.25)); - w->Update(timekeeper); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 5.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 5.0)); - ASSERT_TRUE(fltcmp(b->physics_body_->GetAngle(), 0.5)); - w->Update(timekeeper); - - delete w; -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "tween_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name tween_test.cpp + * @brief test tween test plugin + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; +using namespace flatland_plugins; + +class TweenPluginTest : public ::testing::Test { + public: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + } + + static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { + if (std::isinf(n1) && std::isinf(n2)) { + return true; + } + + if (std::isnan(n1) && std::isnan(n2)) { + return true; + } + + bool ret = fabs(n1 - n2) < epsilon; + return ret; + } +}; + +/** + * Test the tween plugin handles oneshot + */ +TEST_F(TweenPluginTest, once_test) { + world_yaml = this_file_dir / fs::path("tween_tests/once.world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(0.5); + World* w = World::MakeWorld(world_yaml.string()); + + Tween* tween = + dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + + Body* b = tween->body_; + + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 2.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 1.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 2.5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 2.5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 1.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 3.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 4.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 2.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 3.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 4.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 2.0)); + + delete w; +} + +/** + * Test that the tween plugin yoyos + */ +TEST_F(TweenPluginTest, yoyo_test) { + world_yaml = this_file_dir / fs::path("tween_tests/yoyo.world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(0.5); + World* w = World::MakeWorld(world_yaml.string()); + + Tween* tween = + dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + + Body* b = tween->body_; + + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 0.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 0.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.5)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 10.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 10.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 1.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.5)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 0.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 0.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.5)); + w->Update(timekeeper); + + delete w; +} + +/** + * Test that the tween plugin loops + */ +TEST_F(TweenPluginTest, loop_test) { + world_yaml = this_file_dir / fs::path("tween_tests/loop.world.yaml"); + + Timekeeper timekeeper; + timekeeper.SetMaxStepSize(0.5); + World* w = World::MakeWorld(world_yaml.string()); + + Tween* tween = + dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + + Body* b = tween->body_; + + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 0.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 0.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 2.5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 2.5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.25)); + w->Update(timekeeper); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 7.5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 7.5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.75)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 10)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 10)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 1.0)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 2.5)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 2.5)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.25)); + w->Update(timekeeper); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).x, 5.0)); + ASSERT_TRUE(fltcmp(b2Body_GetPosition(b->physics_body_).y, 5.0)); + ASSERT_TRUE(fltcmp(b2Rot_GetAngle(b2Body_GetRotation(b->physics_body_)), 0.5)); + w->Update(timekeeper); + + delete w; +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "tween_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/tween_tests/loop.world.yaml b/flatland_plugins/test/tween_tests/loop.world.yaml index 0735c0c6..58802cc3 100644 --- a/flatland_plugins/test/tween_tests/loop.world.yaml +++ b/flatland_plugins/test/tween_tests/loop.world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/tween_tests/loop.world_plugins.yaml b/flatland_plugins/test/tween_tests/loop.world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/tween_tests/loop.world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/tween_tests/once.world.yaml b/flatland_plugins/test/tween_tests/once.world.yaml index 5de7800a..cbd543cc 100644 --- a/flatland_plugins/test/tween_tests/once.world.yaml +++ b/flatland_plugins/test/tween_tests/once.world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/tween_tests/once.world_plugins.yaml b/flatland_plugins/test/tween_tests/once.world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/tween_tests/once.world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/tween_tests/yoyo.world.yaml b/flatland_plugins/test/tween_tests/yoyo.world.yaml index 0b0f1295..a5238fb5 100644 --- a/flatland_plugins/test/tween_tests/yoyo.world.yaml +++ b/flatland_plugins/test/tween_tests/yoyo.world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/tween_tests/yoyo.world_plugins.yaml b/flatland_plugins/test/tween_tests/yoyo.world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/tween_tests/yoyo.world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp index bd5824dc..d650ef68 100644 --- a/flatland_plugins/test/update_timer_test.cpp +++ b/flatland_plugins/test/update_timer_test.cpp @@ -1,222 +1,222 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name laser_test.cpp - * @brief test laser plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; -using namespace flatland_plugins; - -class TestPlugin : public ModelPlugin { - public: - UpdateTimer update_timer_; - int update_counter_; - - void OnInitialize(const YAML::Node& config) override { - update_timer_.SetRate(0); - update_counter_ = 0; - } - - void BeforePhysicsStep(const Timekeeper& timekeeper) override { - // keeps this function updating at a specific rate - if (!update_timer_.CheckUpdate(timekeeper)) { - return; - } - update_counter_++; - } -}; - -class UpdateTimerTest : public ::testing::Test { - public: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - double set_rate; - double expected_rate; - double actual_rate; - double wall_rate; - double step_size; - double sim_test_time; - World* w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - void ExecuteRateTest() { - Timekeeper timekeeper; - w = World::MakeWorld(world_yaml.string()); - - // artificially load a plugin - boost::shared_ptr p(new TestPlugin()); - p->Initialize("TestPlugin", "test_plugin", w->models_[0], YAML::Node()); - w->plugin_manager_.model_plugins_.push_back(p); - - p->update_timer_.SetRate(set_rate); - - timekeeper.SetMaxStepSize(step_size); - ros::WallRate rate(wall_rate); - - // run for two seconds - while (timekeeper.GetSimTime() < ros::Time(sim_test_time)) { - w->Update(timekeeper); - ros::spinOnce(); - rate.sleep(); - } - - actual_rate = p->update_counter_ / timekeeper.GetSimTime().toSec(); - - printf("Actual Rate: %f, Expected Rate: %f\n", actual_rate, expected_rate); - } -}; - -/** - * Test update rate at real time factor > 1 - */ -TEST_F(UpdateTimerTest, rate_test_A) { - world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); - set_rate = 141.56; - expected_rate = set_rate; - sim_test_time = 2.0; - - // This makes the simulation run at 2.36936936937 real time speed - wall_rate = 789.0; - step_size = 1 / 333.0; - - ExecuteRateTest(); - - EXPECT_NEAR(actual_rate, expected_rate, 1); -} - -/** - * Test update rate at real time factor < 1 - */ -TEST_F(UpdateTimerTest, rate_test_B) { - world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); - set_rate = 564.56; - expected_rate = set_rate; - sim_test_time = 1.0; - - // This makes the simulation run at 0.179902755 real time speed - wall_rate = 222.0; - step_size = 1 / 1234.0; - - ExecuteRateTest(); - - EXPECT_NEAR(actual_rate, expected_rate, 1); -} - -/** - * Test update rate at real time factor >> 1 - */ -TEST_F(UpdateTimerTest, rate_test_C) { - world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); - set_rate = 47.4; - expected_rate = set_rate; - sim_test_time = 2; - - // This makes the simulation run at 10x real time speed - wall_rate = 1000.0; - step_size = 1 / 100.0; - - ExecuteRateTest(); - - EXPECT_NEAR(actual_rate, expected_rate, 1); -} - -/** - * Test update rate at update rate = inf, which will update as fast as possible - */ -TEST_F(UpdateTimerTest, rate_test_D) { - world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); - set_rate = std::numeric_limits::infinity(); - expected_rate = 100.0; - sim_test_time = 2; - - // This makes the simulation run at 10x real time speed - wall_rate = 1000.0; - step_size = 1 / 100.0; - - ExecuteRateTest(); - - EXPECT_NEAR(actual_rate, expected_rate, 1); -} - -/** - * Test update rate at update rate = 0, which will never update - */ -TEST_F(UpdateTimerTest, rate_test_E) { - world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); - set_rate = 0; - expected_rate = 0; - sim_test_time = 2; - - // This makes the simulation run at 10x real time speed - wall_rate = 1000.0; - step_size = 1 / 100.0; - - ExecuteRateTest(); - - EXPECT_NEAR(actual_rate, expected_rate, 1); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "model_tf_plugin_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name laser_test.cpp + * @brief test laser plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; +using namespace flatland_plugins; + +class TestPlugin : public ModelPlugin { + public: + UpdateTimer update_timer_; + int update_counter_; + + void OnInitialize(const YAML::Node& config) override { + update_timer_.SetRate(0); + update_counter_ = 0; + } + + void BeforePhysicsStep(const Timekeeper& timekeeper) override { + // keeps this function updating at a specific rate + if (!update_timer_.CheckUpdate(timekeeper)) { + return; + } + update_counter_++; + } +}; + +class UpdateTimerTest : public ::testing::Test { + public: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + double set_rate; + double expected_rate; + double actual_rate; + double wall_rate; + double step_size; + double sim_test_time; + World* w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + void ExecuteRateTest() { + Timekeeper timekeeper; + w = World::MakeWorld(world_yaml.string()); + + // artificially load a plugin + boost::shared_ptr p(new TestPlugin()); + p->Initialize("TestPlugin", "test_plugin", w->models_[0], YAML::Node()); + w->plugin_manager_.model_plugins_.push_back(p); + + p->update_timer_.SetRate(set_rate); + + timekeeper.SetMaxStepSize(step_size); + ros::WallRate rate(wall_rate); + + // run for two seconds + while (timekeeper.GetSimTime() < ros::Time(sim_test_time)) { + w->Update(timekeeper); + ros::spinOnce(); + rate.sleep(); + } + + actual_rate = p->update_counter_ / timekeeper.GetSimTime().toSec(); + + printf("Actual Rate: %f, Expected Rate: %f\n", actual_rate, expected_rate); + } +}; + +/** + * Test update rate at real time factor > 1 + */ +TEST_F(UpdateTimerTest, rate_test_A) { + world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); + set_rate = 141.56; + expected_rate = set_rate; + sim_test_time = 2.0; + + // This makes the simulation run at 2.36936936937 real time speed + wall_rate = 789.0; + step_size = 1 / 333.0; + + ExecuteRateTest(); + + EXPECT_NEAR(actual_rate, expected_rate, 1); +} + +/** + * Test update rate at real time factor < 1 + */ +TEST_F(UpdateTimerTest, rate_test_B) { + world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); + set_rate = 564.56; + expected_rate = set_rate; + sim_test_time = 1.0; + + // This makes the simulation run at 0.179902755 real time speed + wall_rate = 222.0; + step_size = 1 / 1234.0; + + ExecuteRateTest(); + + EXPECT_NEAR(actual_rate, expected_rate, 1); +} + +/** + * Test update rate at real time factor >> 1 + */ +TEST_F(UpdateTimerTest, rate_test_C) { + world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); + set_rate = 47.4; + expected_rate = set_rate; + sim_test_time = 2; + + // This makes the simulation run at 10x real time speed + wall_rate = 1000.0; + step_size = 1 / 100.0; + + ExecuteRateTest(); + + EXPECT_NEAR(actual_rate, expected_rate, 1); +} + +/** + * Test update rate at update rate = inf, which will update as fast as possible + */ +TEST_F(UpdateTimerTest, rate_test_D) { + world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); + set_rate = std::numeric_limits::infinity(); + expected_rate = 100.0; + sim_test_time = 2; + + // This makes the simulation run at 10x real time speed + wall_rate = 1000.0; + step_size = 1 / 100.0; + + ExecuteRateTest(); + + EXPECT_NEAR(actual_rate, expected_rate, 1); +} + +/** + * Test update rate at update rate = 0, which will never update + */ +TEST_F(UpdateTimerTest, rate_test_E) { + world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); + set_rate = 0; + expected_rate = 0; + sim_test_time = 2; + + // This makes the simulation run at 10x real time speed + wall_rate = 1000.0; + step_size = 1 / 100.0; + + ExecuteRateTest(); + + EXPECT_NEAR(actual_rate, expected_rate, 1); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "model_tf_plugin_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_plugins/test/update_timer_test/world.yaml b/flatland_plugins/test/update_timer_test/world.yaml index ae449cba..f82f2010 100644 --- a/flatland_plugins/test/update_timer_test/world.yaml +++ b/flatland_plugins/test/update_timer_test/world.yaml @@ -1,5 +1,4 @@ -properties: {} -layers: +layers: - name: "layer_1" map: "map_1.yaml" color: [0, 1, 0, 1] diff --git a/flatland_plugins/test/update_timer_test/world_plugins.yaml b/flatland_plugins/test/update_timer_test/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_plugins/test/update_timer_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/CHANGELOG.rst b/flatland_server/CHANGELOG.rst new file mode 100644 index 00000000..405af851 --- /dev/null +++ b/flatland_server/CHANGELOG.rst @@ -0,0 +1,28 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package flatland_server +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.1 (2023-11-24) +------------------ +* CMake version required bump to fix ros build farm warning + +1.4.0 (2023-11-22) +------------------ +* Version Bump + +1.3.3 (2023-02-06) +------------------ +* added missing std_srvs dep +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* added missing std_srvs dep +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* added missing std_srvs dep +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 59711c12..5940f1c6 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -1,8 +1,9 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.11) project(flatland_server) -# Ensure we're using c++11 -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +# Flatland server uses C++17 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -43,6 +44,33 @@ find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS date_time system thread) find_package(Threads) +# Box2D v3 + enkiTS via FetchContent +include(FetchContent) + +FetchContent_Declare( + box2d + GIT_REPOSITORY https://github.com/erincatto/box2d.git + GIT_TAG v3.1.0 +) +# Disable Box2D samples/tests/benchmarks to avoid needing OpenGL +set(BOX2D_SAMPLES OFF CACHE BOOL "" FORCE) +set(BOX2D_BENCHMARKS OFF CACHE BOOL "" FORCE) +set(BOX2D_UNIT_TESTS OFF CACHE BOOL "" FORCE) +# Build dependencies with PIC for shared library linking +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +FetchContent_MakeAvailable(box2d) +# Box2D v3.1.0 distance.c triggers -Werror=maybe-uninitialized on GCC (false positive) +target_compile_options(box2d PRIVATE -Wno-maybe-uninitialized) + +FetchContent_Declare( + enkiTS + GIT_REPOSITORY https://github.com/dougbinks/enkiTS.git + GIT_TAG v1.11 +) +FetchContent_MakeAvailable(enkiTS) +# enkiTS uses include_directories() which doesn't propagate; add explicitly +include_directories(${enkits_SOURCE_DIR}/src) + ############## ## coverage ## ############## @@ -58,8 +86,8 @@ endif() ## catkin specific configuration ## ################################### catkin_package( - INCLUDE_DIRS include thirdparty - LIBRARIES flatland_lib flatland_Box2D + INCLUDE_DIRS include ${box2d_SOURCE_DIR}/include ${enkits_SOURCE_DIR}/src + LIBRARIES flatland_lib CATKIN_DEPENDS pluginlib roscpp std_msgs tf2 visualization_msgs tf2_geometry_msgs geometry_msgs DEPENDS OpenCV YAML_CPP ) @@ -75,11 +103,8 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${LUA_INCLUDE_DIR} - thirdparty ) -add_subdirectory("thirdparty/Box2D") - ## Flatland server library add_library(flatland_lib src/simulation_manager.cpp @@ -103,6 +128,7 @@ add_library(flatland_lib src/dummy_model_plugin.cpp src/dummy_world_plugin.cpp src/yaml_preprocessor.cpp + src/message_server.cpp ) add_dependencies(flatland_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -111,7 +137,8 @@ target_link_libraries(flatland_lib ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ${LUA_LIBRARIES} - flatland_Box2D + box2d + enkiTS yaml-cpp ) @@ -130,7 +157,8 @@ target_link_libraries(flatland_server ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ${LUA_LIBRARIES} - flatland_Box2D + box2d + enkiTS yaml-cpp flatland_lib ) @@ -162,6 +190,16 @@ install(DIRECTORY include/${PROJECT_NAME}/ FILES_MATCHING PATTERN "*.h" ) +# Install enkiTS headers alongside flatland_server headers. +# world.h includes "TaskScheduler.h" with a relative include, so these must be +# co-located with the installed flatland_server headers. +install(FILES + ${enkits_SOURCE_DIR}/src/TaskScheduler.h + ${enkits_SOURCE_DIR}/src/LockLessMultiReadPipe.h + include/${PROJECT_NAME}/message_server.h + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + # Install launchfiles install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/flatland_server/LICENSE b/flatland_server/LICENSE new file mode 100644 index 00000000..32802b81 --- /dev/null +++ b/flatland_server/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, Avidbots Corp. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/flatland_server/include/flatland_server/body.h b/flatland_server/include/flatland_server/body.h index a0d00ed3..ecbb0fb9 100644 --- a/flatland_server/include/flatland_server/body.h +++ b/flatland_server/include/flatland_server/body.h @@ -1,137 +1,145 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name body.h - * @brief Defines Body - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_BODY_H -#define FLATLAND_BODY_H - -#include -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a body in the simulation. It wraps around the Box2D - * physics body providing extra data and useful methods - */ -class Body { - public: - Entity *entity_; ///< The entity the body belongs to - std::string name_; ///< name of the body, unique within a model - b2Body *physics_body_; ///< Box2D physics body - Color color_; ///< color, for visualization - YAML::Node properties_; ///< Properties document for plugins to use - - /** - * @brief constructor for body, takes in all the required parameters - * @param[in] physics_world Box2D physics world - * @param[in] entity Entity the body is tied to - * @param[in] name Name for the body - * @param[in] color Color in r, g, b, a - * @param[in] pose Pose to place the body at - * @param[in] body_type Box2D body type either dynamic, kinematic, or static - * @param[in] properties per-object YAML properties for plugins to reference - * @param[in] linear_damping Box2D body linear damping - * @param[in] angular_damping Box2D body angular damping - */ - Body(b2World *physics_world, Entity *entity, const std::string &name, - const Color &color, const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping = 0, - double angular_damping = 0); - - /** - * @brief logs the debugging information for the body - */ - void DebugOutput() const; - - /** - * @return entity associated with the body - */ - Entity *GetEntity(); - - /** - * @return name of the body - */ - const std::string &GetName() const; - - /** - * @brief Get the Box2D body, use this to manipulate the body in physics - * through the Box2D methods - * @return Pointer to Box2D physics body - */ - b2Body *GetPhysicsBody(); - - /** - * @brief Count the number of fixtures - * @return number of fixtures in the body - */ - int GetFixturesCount() const; - - /** - * @return Color of the body - */ - const Color &GetColor() const; - - /** - * @brief Set of the color of the body - */ - void SetColor(const Color &color); - - /** - * Destructor for the body - */ - virtual ~Body(); - - /** - * Prevent copying since it is creates complications with constructing - * and destructing bodies - */ - Body(const Body &) = delete; - Body &operator=(const Body &) = delete; -}; -}; // namespace flatland_server -#endif // FLATLAND_MODEL_BODY_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name body.h + * @brief Defines Body + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_BODY_H +#define FLATLAND_BODY_H + +#include +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a body in the simulation. It wraps around the Box2D + * physics body providing extra data and useful methods + */ +class Body { + public: + Entity *entity_; ///< The entity the body belongs to + std::string name_; ///< name of the body, unique within a model + b2BodyId physics_body_; ///< Box2D v3 physics body ID + Color color_; ///< color, for visualization + YAML::Node properties_; ///< Properties document for plugins to use + Pose parent_transform_; ///< Initial pose (offset from parent entity) + + /** + * @brief constructor for body, takes in all the required parameters + * @param[in] physics_world Box2D physics world + * @param[in] entity Entity the body is tied to + * @param[in] name Name for the body + * @param[in] color Color in r, g, b, a + * @param[in] pose Pose to place the body at + * @param[in] body_type Box2D body type either dynamic, kinematic, or static + * @param[in] properties per-object YAML properties for plugins to reference + * @param[in] linear_damping Box2D body linear damping + * @param[in] angular_damping Box2D body angular damping + */ + Body(b2WorldId physics_world, Entity *entity, const std::string &name, + const Color &color, const Pose &pose, b2BodyType body_type, + const YAML::Node &properties, double linear_damping = 0, + double angular_damping = 0); + + /** + * @brief logs the debugging information for the body + */ + void DebugOutput() const; + + /** + * @return entity associated with the body + */ + Entity *GetEntity(); + + /** + * @return name of the body + */ + const std::string &GetName() const; + + /** + * @brief Get the Box2D body, use this to manipulate the body in physics + * through the Box2D methods + * @return Pointer to Box2D physics body + */ + b2BodyId GetPhysicsBody(); + + /** + * @brief Get the initial pose (parent transform) of this body, as set at + * construction. Used to compute static sensor-to-body transforms. + * @return Initial pose {x, y, theta} + */ + const Pose &GetParentTransform() const { return parent_transform_; } + + /** + * @brief Count the number of fixtures + * @return number of fixtures in the body + */ + int GetShapesCount() const; + + /** + * @return Color of the body + */ + const Color &GetColor() const; + + /** + * @brief Set of the color of the body + */ + void SetColor(const Color &color); + + /** + * Destructor for the body + */ + virtual ~Body(); + + /** + * Prevent copying since it is creates complications with constructing + * and destructing bodies + */ + Body(const Body &) = delete; + Body &operator=(const Body &) = delete; +}; +}; // namespace flatland_server +#endif // FLATLAND_MODEL_BODY_H diff --git a/flatland_server/include/flatland_server/collision_filter_registry.h b/flatland_server/include/flatland_server/collision_filter_registry.h index 866f403d..ae127dcb 100644 --- a/flatland_server/include/flatland_server/collision_filter_registry.h +++ b/flatland_server/include/flatland_server/collision_filter_registry.h @@ -1,136 +1,136 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name collision_filter_registry.h - * @brief Defines Collision Filter Registrar - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H -#define FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H - -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a collision filter registry. It allows layers to register - * for unique ID's, which are used for setting Box2D collision categories - * and mask bits. The footprints in the model will use this to put itself on - * the correct collision category. It also hands out unique collide (positive - * numbers) and no collide (negative numbers) Box2D collision groups. - */ -class CollisionFilterRegistry { - public: - static const int LAYER_NOT_EXIST = -1; ///< No such layer - static const int LAYER_ALREADY_EXIST = -2; ///< Layer exists - static const int LAYERS_FULL = -3; ///< Cannot add more layers - static const int MAX_LAYERS = 16; ///< 16 is the maximum as defined by Box2D - - /// internal counter to keep track of no collides groups - int no_collide_group_cnt_; - /// internal counter to keep track of collide groups - int collide_group_cnt_; - std::map layer_id_table_; ///< Layer name to ID LUT - - /** - * @brief Constructor for the collision filter registry - */ - CollisionFilterRegistry(); - - /** - * @brief Get a new and unique collision group, +ve numbers - */ - int RegisterCollide(); - - /** - * @brief Get a new and unique no collision group, -ve numbers - */ - int RegisterNoCollide(); - - /** - * @brief Check if the number of layers maxed out - * @return if layers are full - */ - bool IsLayersFull() const; - - /** - * @brief Register a new layer - * @param[in] layer Name of the layer - * @return assigned ID for the registered layer, or error codes LAYERS_FULL or - * LAYER_ALREADY_EXIST - */ - int RegisterLayer(std::string layer); - - /** - * @brief get layer ID - * @param[in] name Name of the layer - * @return the id of the layer, or LAYER_NOT_EXIST - */ - int LookUpLayerId(std::string name) const; - - /** - * @brief Get all registered layers - * @return vector to store layer names in, will be cleared - */ - std::vector GetAllLayers() const; - - /** - * @brief Get number of layers - * @return number of layers - */ - int LayersCount() const; - - /** - * @brief: Get the Box2D category bits from a list of layers - * @param[in] layers The layers for generating the category bits, if the input - * exactly equals to {"all"}, it returns all bits to 1 (0xFFFF) - * @param[out] invalid_layers if a given layer does not exist, it is pushed to - * this list, optional - */ - uint16_t GetCategoryBits( - const std::vector &layers, - std::vector *invalid_layers = nullptr) const; -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name collision_filter_registry.h + * @brief Defines Collision Filter Registrar + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H +#define FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H + +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a collision filter registry. It allows layers to register + * for unique ID's, which are used for setting Box2D collision categories + * and mask bits. The footprints in the model will use this to put itself on + * the correct collision category. It also hands out unique collide (positive + * numbers) and no collide (negative numbers) Box2D collision groups. + */ +class CollisionFilterRegistry { + public: + static const int LAYER_NOT_EXIST = -1; ///< No such layer + static const int LAYER_ALREADY_EXIST = -2; ///< Layer exists + static const int LAYERS_FULL = -3; ///< Cannot add more layers + static const int MAX_LAYERS = 16; ///< 16 is the maximum as defined by Box2D + + /// internal counter to keep track of no collides groups + int no_collide_group_cnt_; + /// internal counter to keep track of collide groups + int collide_group_cnt_; + std::map layer_id_table_; ///< Layer name to ID LUT + + /** + * @brief Constructor for the collision filter registry + */ + CollisionFilterRegistry(); + + /** + * @brief Get a new and unique collision group, +ve numbers + */ + int RegisterCollide(); + + /** + * @brief Get a new and unique no collision group, -ve numbers + */ + int RegisterNoCollide(); + + /** + * @brief Check if the number of layers maxed out + * @return if layers are full + */ + bool IsLayersFull() const; + + /** + * @brief Register a new layer + * @param[in] layer Name of the layer + * @return assigned ID for the registered layer, or error codes LAYERS_FULL or + * LAYER_ALREADY_EXIST + */ + int RegisterLayer(std::string layer); + + /** + * @brief get layer ID + * @param[in] name Name of the layer + * @return the id of the layer, or LAYER_NOT_EXIST + */ + int LookUpLayerId(std::string name) const; + + /** + * @brief Get all registered layers + * @return vector to store layer names in, will be cleared + */ + std::vector GetAllLayers() const; + + /** + * @brief Get number of layers + * @return number of layers + */ + int LayersCount() const; + + /** + * @brief: Get the Box2D category bits from a list of layers + * @param[in] layers The layers for generating the category bits, if the input + * exactly equals to {"all"}, it returns all bits to 1 (0xFFFF) + * @param[out] invalid_layers if a given layer does not exist, it is pushed to + * this list, optional + */ + uint16_t GetCategoryBits( + const std::vector &layers, + std::vector *invalid_layers = nullptr) const; +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H diff --git a/flatland_server/include/flatland_server/debug_visualization.h b/flatland_server/include/flatland_server/debug_visualization.h index 46b46439..6e052f94 100644 --- a/flatland_server/include/flatland_server/debug_visualization.h +++ b/flatland_server/include/flatland_server/debug_visualization.h @@ -1,165 +1,165 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name debug_visualization.h - * @brief Transform box2d types into published visualization messages - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_DEBUG_VISUALIZATION_H -#define FLATLAND_SERVER_DEBUG_VISUALIZATION_H - -#include -#include -#include -#include -#include -#include -#include - -#include "flatland_server/body.h" -#include "flatland_server/timekeeper.h" - -namespace flatland_server { -struct DebugTopic { - ros::Publisher publisher; - bool needs_publishing; - visualization_msgs::MarkerArray markers; -}; - -class DebugVisualization { - private: - DebugVisualization(); - - public: - std::map topics_; - ros::NodeHandle node_; - ros::Publisher topic_list_publisher_; - - /** - * @brief Return the singleton object - */ - static DebugVisualization& Get(); - - /** - * @brief Publish all marker array topics_ that need publishing - * @param[in] timekeeper The time object to use for header timestamps - */ - void Publish(const Timekeeper& timekeeper); - - /** - * @brief Visualize body - * @param[in] name The name of the topic - * @param[in] body The body to output - * @param[in] r red color 0.0->1.0 - * @param[in] g green color 0.0->1.0 - * @param[in] b blue color 0.0->1.0 - * @param[in] a alpha color 0.0->1.0 - */ - void Visualize(std::string name, b2Body* body, float r, float g, float b, - float a); - - /** - * @brief Visualize body - * @param[in] name The name of the topic - * @param[in] joint The join to output - * @param[in] r red color 0.0->1.0 - * @param[in] g green color 0.0->1.0 - * @param[in] b blue color 0.0->1.0 - * @param[in] a alpha color 0.0->1.0 - */ - void Visualize(std::string name, b2Joint* joint, float r, float g, float b, - float a); - - /** - * @brief Visualize a layer in 2.5d - * @param[in] name The name of the topic - * @param[in] joint The join to output - * @param[in] r red color 0.0->1.0 - * @param[in] g green color 0.0->1.0 - * @param[in] b blue color 0.0->1.0 - * @param[in] a alpha color 0.0->1.0 - */ - void VisualizeLayer(std::string name, Body* body); - - /** - * @brief Remove all elements in a visualization topic - * @param name - */ - void Reset(std::string name); - - /** - * @brief Append body as a marker on the marker array - * @param[in] markers The output marker array - * @param[in] body The input body pointer - * @param[in] r red color 0.0->1.0 - * @param[in] g green color 0.0->1.0 - * @param[in] b blue color 0.0->1.0 - * @param[in] a alpha color 0.0->1.0 - */ - void BodyToMarkers(visualization_msgs::MarkerArray& markers, b2Body* body, - float r, float g, float b, float a); - - /** - * @brief Append a joint as a marker on the marker array - * @param[in] markers The output marker array - * @param[in] joint The input joint pointer - * @param[in] r red color 0.0->1.0 - * @param[in] g green color 0.0->1.0 - * @param[in] b blue color 0.0->1.0 - * @param[in] a alpha color 0.0->1.0 - */ - void JointToMarkers(visualization_msgs::MarkerArray& markers, b2Joint* joint, - float r, float g, float b, float a); - - /** - * @brief Ensure that a topic name is being broadcasted - * @param[in] name Name of the topic - */ - void AddTopicIfNotExist(const std::string& name); - - /** - * @brief Publish topics list - */ - void PublishTopicList(); -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_DEBUG_VISUALIZATION_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name debug_visualization.h + * @brief Transform box2d types into published visualization messages + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_DEBUG_VISUALIZATION_H +#define FLATLAND_SERVER_DEBUG_VISUALIZATION_H + +#include +#include +#include +#include +#include +#include +#include + +#include "flatland_server/body.h" +#include "flatland_server/timekeeper.h" + +namespace flatland_server { +struct DebugTopic { + ros::Publisher publisher; + bool needs_publishing; + visualization_msgs::MarkerArray markers; +}; + +class DebugVisualization { + private: + DebugVisualization(); + + public: + std::map topics_; + ros::NodeHandle node_; + ros::Publisher topic_list_publisher_; + + /** + * @brief Return the singleton object + */ + static DebugVisualization& Get(); + + /** + * @brief Publish all marker array topics_ that need publishing + * @param[in] timekeeper The time object to use for header timestamps + */ + void Publish(const Timekeeper& timekeeper); + + /** + * @brief Visualize body + * @param[in] name The name of the topic + * @param[in] body The body to output + * @param[in] r red color 0.0->1.0 + * @param[in] g green color 0.0->1.0 + * @param[in] b blue color 0.0->1.0 + * @param[in] a alpha color 0.0->1.0 + */ + void Visualize(std::string name, b2BodyId body, float r, float g, float b, + float a); + + /** + * @brief Visualize body + * @param[in] name The name of the topic + * @param[in] joint The join to output + * @param[in] r red color 0.0->1.0 + * @param[in] g green color 0.0->1.0 + * @param[in] b blue color 0.0->1.0 + * @param[in] a alpha color 0.0->1.0 + */ + void Visualize(std::string name, b2JointId joint, float r, float g, float b, + float a); + + /** + * @brief Visualize a layer in 2.5d + * @param[in] name The name of the topic + * @param[in] joint The join to output + * @param[in] r red color 0.0->1.0 + * @param[in] g green color 0.0->1.0 + * @param[in] b blue color 0.0->1.0 + * @param[in] a alpha color 0.0->1.0 + */ + void VisualizeLayer(std::string name, Body* body); + + /** + * @brief Remove all elements in a visualization topic + * @param name + */ + void Reset(std::string name); + + /** + * @brief Append body as a marker on the marker array + * @param[in] markers The output marker array + * @param[in] body The input body pointer + * @param[in] r red color 0.0->1.0 + * @param[in] g green color 0.0->1.0 + * @param[in] b blue color 0.0->1.0 + * @param[in] a alpha color 0.0->1.0 + */ + void BodyToMarkers(visualization_msgs::MarkerArray& markers, b2BodyId body, + float r, float g, float b, float a); + + /** + * @brief Append a joint as a marker on the marker array + * @param[in] markers The output marker array + * @param[in] joint The input joint pointer + * @param[in] r red color 0.0->1.0 + * @param[in] g green color 0.0->1.0 + * @param[in] b blue color 0.0->1.0 + * @param[in] a alpha color 0.0->1.0 + */ + void JointToMarkers(visualization_msgs::MarkerArray& markers, b2JointId joint, + float r, float g, float b, float a); + + /** + * @brief Ensure that a topic name is being broadcasted + * @param[in] name Name of the topic + */ + void AddTopicIfNotExist(const std::string& name); + + /** + * @brief Publish topics list + */ + void PublishTopicList(); +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_DEBUG_VISUALIZATION_H diff --git a/flatland_server/include/flatland_server/dummy_model_plugin.h b/flatland_server/include/flatland_server/dummy_model_plugin.h index 7e22b661..16d6c72e 100644 --- a/flatland_server/include/flatland_server/dummy_model_plugin.h +++ b/flatland_server/include/flatland_server/dummy_model_plugin.h @@ -1,78 +1,78 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name dummy_model_plugin.h - * @brief Dummy model plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_DUMMY_MODEL_PLUGIN_H -#define FLATLAND_PLUGINS_DUMMY_MODEL_PLUGIN_H - -using namespace flatland_server; - -namespace flatland_plugins { -/** - * This is a dummy plugin of type model plugin, used completely for testing - * purposes - */ -class DummyModelPlugin : public ModelPlugin { - public: - int dummy_param_int_; ///< Iteger variable for testing - std::string dummy_param_string_; ///< String variable for testing - double dummy_param_float_; ///< float variable for testing - - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name dummy_model_plugin.h + * @brief Dummy model plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_DUMMY_MODEL_PLUGIN_H +#define FLATLAND_PLUGINS_DUMMY_MODEL_PLUGIN_H + +using namespace flatland_server; + +namespace flatland_plugins { +/** + * This is a dummy plugin of type model plugin, used completely for testing + * purposes + */ +class DummyModelPlugin : public ModelPlugin { + public: + int dummy_param_int_; ///< Iteger variable for testing + std::string dummy_param_string_; ///< String variable for testing + double dummy_param_float_; ///< float variable for testing + + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; +}; +}; + +#endif diff --git a/flatland_server/include/flatland_server/dummy_world_plugin.h b/flatland_server/include/flatland_server/dummy_world_plugin.h index 89a7201e..b82840b7 100644 --- a/flatland_server/include/flatland_server/dummy_world_plugin.h +++ b/flatland_server/include/flatland_server/dummy_world_plugin.h @@ -1,71 +1,71 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name dummy_world_plugin.h - * @brief Dummy World plugin testing basic function - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -#ifndef FLATLAND_PLUGINS_DUMMY_WORLD_PLUGIN_H -#define FLATLAND_PLUGINS_DUMMY_WORLD_PLUGIN_H - -using namespace flatland_server; - -namespace flatland_plugins { -/** - * This is a dummy plugin of type world plugin, used completely for testing - * purposes - */ -class DummyWorldPlugin : public WorldPlugin { - public: - /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; -}; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name dummy_world_plugin.h + * @brief Dummy World plugin testing basic function + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#ifndef FLATLAND_PLUGINS_DUMMY_WORLD_PLUGIN_H +#define FLATLAND_PLUGINS_DUMMY_WORLD_PLUGIN_H + +using namespace flatland_server; + +namespace flatland_plugins { +/** + * This is a dummy plugin of type world plugin, used completely for testing + * purposes + */ +class DummyWorldPlugin : public WorldPlugin { + public: + /** + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node &config) override; +}; +}; + +#endif diff --git a/flatland_server/include/flatland_server/entity.h b/flatland_server/include/flatland_server/entity.h index 2b559834..71fb232c 100644 --- a/flatland_server/include/flatland_server/entity.h +++ b/flatland_server/include/flatland_server/entity.h @@ -1,111 +1,111 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name entity.h - * @brief Defines flatland Entity - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_ENTITY_H -#define FLATLAND_SERVER_ENTITY_H - -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a entity in the simulation world. It provides a class - * for high level physical things in the world to inherit from (layers and - * models) - */ -class Entity { - public: - /// Defines the type of entity - enum EntityType { LAYER, MODEL }; - - b2World *physics_world_; ///< Box2D physics world - std::string name_; ///< name of the entity - - /** - * @brief Constructor for the entity - * @param[in] physics_world Box2D physics_world - * @param[in] name name of the entity - */ - Entity(b2World *physics_world, const std::string &name); - virtual ~Entity() = default; - - /** - * @return name of the entity - */ - const std::string &GetName() const; - - /** - * @brief Get Box2D physics world - * @return Pointer to Box2D physics world, use this to call Box2D world - * methods - */ - b2World *GetPhysicsWorld(); - - /// This class should be non-copyable. This will cause the destructor to be - /// called twice for a given b2Body - Entity(const Entity &) = delete; - Entity &operator=(const Entity &) = delete; - - /** - * @brief Get the type of entity, subclasses must override - * @return the type of entity - */ - virtual EntityType Type() const = 0; - - /** - * @brief Visualize the entity - */ - virtual void DebugVisualize() const = 0; - - /** - * @brief Print debug message for the entity - */ - virtual void DebugOutput() const = 0; -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_ENTITY_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name entity.h + * @brief Defines flatland Entity + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_ENTITY_H +#define FLATLAND_SERVER_ENTITY_H + +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a entity in the simulation world. It provides a class + * for high level physical things in the world to inherit from (layers and + * models) + */ +class Entity { + public: + /// Defines the type of entity + enum EntityType { LAYER, MODEL }; + + b2WorldId physics_world_; ///< Box2D v3 world ID + std::string name_; ///< name of the entity + + /** + * @brief Constructor for the entity + * @param[in] physics_world Box2D v3 world ID + * @param[in] name name of the entity + */ + Entity(b2WorldId physics_world, const std::string &name); + virtual ~Entity() = default; + + /** + * @return name of the entity + */ + const std::string &GetName() const; + + /** + * @brief Get Box2D physics world + * @return Pointer to Box2D physics world, use this to call Box2D world + * methods + */ + b2WorldId GetPhysicsWorld(); + + /// This class should be non-copyable. This will cause the destructor to be + /// called twice for a given b2Body + Entity(const Entity &) = delete; + Entity &operator=(const Entity &) = delete; + + /** + * @brief Get the type of entity, subclasses must override + * @return the type of entity + */ + virtual EntityType Type() const = 0; + + /** + * @brief Visualize the entity + */ + virtual void DebugVisualize() const = 0; + + /** + * @brief Print debug message for the entity + */ + virtual void DebugOutput() const = 0; +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_ENTITY_H diff --git a/flatland_server/include/flatland_server/exceptions.h b/flatland_server/include/flatland_server/exceptions.h index d4ee7b1f..edca6ae6 100644 --- a/flatland_server/include/flatland_server/exceptions.h +++ b/flatland_server/include/flatland_server/exceptions.h @@ -1,135 +1,135 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world.h - * @brief Defines flatland Layer - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_EXCEPTIONS_H -#define FLATLAND_SERVER_EXCEPTIONS_H - -#include -#include -#include -#include - -namespace flatland_server { - -class Exception : public std::runtime_error { - public: - /** - * @brief Constructor for the Exception class - */ - Exception(const std::string &msg) : runtime_error(msg) {} -}; - -class PluginException : public Exception { - public: - /** - * @brief Constructor for PluginException - * @param[in] msg custom message - */ - PluginException(const std::string &msg) : Exception(ErrorMsg(msg)) {} - - private: - /** - * @brief Generates exception message for plugin exception - * @param[in] msg Custom error message - */ - static const std::string ErrorMsg(const std::string &msg) { - std::stringstream output; - output << "Flatland plugin: "; - output << msg; - return output.str(); - } -}; - -class YAMLException : public Exception { - public: - /** - * @brief Constructor for the YAMLException class, stores and generates - * exception message using yaml cpp exceptions - * @param[in] msg Exception message - * @param[in] yaml_cpp_exception Exception generated from YAML cpp - */ - YAMLException(const std::string &msg, - const YAML::Exception &yaml_cpp_exception) - : Exception( - ErrorMsg(msg, yaml_cpp_exception.msg, yaml_cpp_exception.mark)) {} - - /** - * @brief Constructor for the YAMLException class, stores and generates - * exception message using just a message - * @param[in] msg Exception message - */ - YAMLException(const std::string &msg) : Exception("Flatland YAML: " + msg) {} - - private: - /** - * @brief Generates exception message from yaml cpp messages - * @param[in] msg Exception message - * @param[in] yaml_cpp_msg Exception message generated by yaml cpp - * @param[in] yaml_cpp_mark Mark generated by yaml cpp - */ - static const std::string ErrorMsg(const std::string &msg, - const std::string &yaml_cpp_msg, - const YAML::Mark &yaml_cpp_mark) { - std::stringstream output; - - output << "Flatland YAML: "; - output << msg; - if (!(yaml_cpp_mark.pos == -1 && yaml_cpp_mark.line == -1 && - yaml_cpp_mark.column == -1)) { - output << ", line " << yaml_cpp_mark.line + 1 << " col " - << yaml_cpp_mark.column + 1; - } - - if (yaml_cpp_msg.size() > 0) { - output << ", " << yaml_cpp_msg; - } - - return output.str(); - } -}; -}; // namespace flatland_server - -#endif // FLATLAND_SERVER_WORLD_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world.h + * @brief Defines flatland Layer + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_EXCEPTIONS_H +#define FLATLAND_SERVER_EXCEPTIONS_H + +#include +#include +#include +#include + +namespace flatland_server { + +class Exception : public std::runtime_error { + public: + /** + * @brief Constructor for the Exception class + */ + Exception(const std::string &msg) : runtime_error(msg) {} +}; + +class PluginException : public Exception { + public: + /** + * @brief Constructor for PluginException + * @param[in] msg custom message + */ + PluginException(const std::string &msg) : Exception(ErrorMsg(msg)) {} + + private: + /** + * @brief Generates exception message for plugin exception + * @param[in] msg Custom error message + */ + static const std::string ErrorMsg(const std::string &msg) { + std::stringstream output; + output << "Flatland plugin: "; + output << msg; + return output.str(); + } +}; + +class YAMLException : public Exception { + public: + /** + * @brief Constructor for the YAMLException class, stores and generates + * exception message using yaml cpp exceptions + * @param[in] msg Exception message + * @param[in] yaml_cpp_exception Exception generated from YAML cpp + */ + YAMLException(const std::string &msg, + const YAML::Exception &yaml_cpp_exception) + : Exception( + ErrorMsg(msg, yaml_cpp_exception.msg, yaml_cpp_exception.mark)) {} + + /** + * @brief Constructor for the YAMLException class, stores and generates + * exception message using just a message + * @param[in] msg Exception message + */ + YAMLException(const std::string &msg) : Exception("Flatland YAML: " + msg) {} + + private: + /** + * @brief Generates exception message from yaml cpp messages + * @param[in] msg Exception message + * @param[in] yaml_cpp_msg Exception message generated by yaml cpp + * @param[in] yaml_cpp_mark Mark generated by yaml cpp + */ + static const std::string ErrorMsg(const std::string &msg, + const std::string &yaml_cpp_msg, + const YAML::Mark &yaml_cpp_mark) { + std::stringstream output; + + output << "Flatland YAML: "; + output << msg; + if (!(yaml_cpp_mark.pos == -1 && yaml_cpp_mark.line == -1 && + yaml_cpp_mark.column == -1)) { + output << ", line " << yaml_cpp_mark.line + 1 << " col " + << yaml_cpp_mark.column + 1; + } + + if (yaml_cpp_msg.size() > 0) { + output << ", " << yaml_cpp_msg; + } + + return output.str(); + } +}; +}; // namespace flatland_server + +#endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/flatland_plugin.h b/flatland_server/include/flatland_server/flatland_plugin.h index 83567f28..7355b3ff 100644 --- a/flatland_server/include/flatland_server/flatland_plugin.h +++ b/flatland_server/include/flatland_server/flatland_plugin.h @@ -1,132 +1,132 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_plugin.h - * @brief Interface for Flatland pluginlib plugins - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_FLATLAND_PLUGIN_H -#define FLATLAND_SERVER_FLATLAND_PLUGIN_H - -#include -#include -#include -#include -#include - -namespace flatland_server { -class FlatlandPlugin { - public: - enum class PluginType { Invalid, Model, World }; // Different plugin Types - std::string type_; ///< type of the plugin - std::string name_; ///< name of the plugin - ros::NodeHandle nh_; // ROS node handle - PluginType plugin_type_; - - /* - * @brief Get PluginType - */ - const PluginType Type() { return plugin_type_; } - - /** - * @brief Get plugin name - */ - const std::string &GetName() const { return name_; } - - /** - * @brief Get type of plugin - */ - const std::string &GetType() const { return type_; } - - /** - * @brief The method for the particular model plugin to override and provide - * its own initialization - * @param[in] config The plugin YAML node - */ - virtual void OnInitialize(const YAML::Node &config) = 0; - - /** - * @brief This method is called before the Box2D physics step - * @param[in] timekeeper provide time related information - */ - virtual void BeforePhysicsStep(const Timekeeper &timekeeper) {} - - /** - * @brief This method is called after the Box2D physics step - * @param[in] timekeeper provide time related information - */ - virtual void AfterPhysicsStep(const Timekeeper &timekeeper) {} - - /** - * @brief A method that is called for all Box2D begin contacts - * @param[in] contact Box2D contact - */ - virtual void BeginContact(b2Contact *contact) {} - - /** - * @brief A method that is called for all Box2D end contacts - * @param[in] contact Box2D contact - */ - virtual void EndContact(b2Contact *contact) {} - - /** - * @brief A method that is called for Box2D presolve - * @param[in] contact Box2D contact - * @param[in] oldManifold Manifold from the previous iteration - */ - virtual void PreSolve(b2Contact *contact, const b2Manifold *oldManifold) {} - - /** - * @brief A method that is called for Box2D postsolve - * @param[in] contact Box2D contact - * @param[in] impulse Impulse from the collision resolution - */ - virtual void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) {} - - /** - * @brief Flatland plugin destructor - */ - virtual ~FlatlandPlugin() = default; -}; -}; // namespace - -#endif // FLATLAND_SERVER_FLATLAND_PLUGIN_H \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_plugin.h + * @brief Interface for Flatland pluginlib plugins + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_FLATLAND_PLUGIN_H +#define FLATLAND_SERVER_FLATLAND_PLUGIN_H + +#include +#include +#include +#include +#include + +namespace flatland_server { +class FlatlandPlugin { + public: + enum class PluginType { Invalid, Model, World }; // Different plugin Types + std::string type_; ///< type of the plugin + std::string name_; ///< name of the plugin + ros::NodeHandle nh_; // ROS node handle + PluginType plugin_type_; + + /* + * @brief Get PluginType + */ + const PluginType Type() { return plugin_type_; } + + /** + * @brief Get plugin name + */ + const std::string &GetName() const { return name_; } + + /** + * @brief Get type of plugin + */ + const std::string &GetType() const { return type_; } + + /** + * @brief The method for the particular model plugin to override and provide + * its own initialization + * @param[in] config The plugin YAML node + */ + virtual void OnInitialize(const YAML::Node &config) = 0; + + /** + * @brief This method is called before the Box2D physics step + * @param[in] timekeeper provide time related information + */ + virtual void BeforePhysicsStep(const Timekeeper &timekeeper) {} + + /** + * @brief This method is called after the Box2D physics step + * @param[in] timekeeper provide time related information + */ + virtual void AfterPhysicsStep(const Timekeeper &timekeeper) {} + + /** + * @brief A method that is called for all Box2D begin contacts + * @param[in] shapeIdA First shape in contact + * @param[in] shapeIdB Second shape in contact + */ + virtual void BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) {} + + /** + * @brief A method that is called for all Box2D end contacts + * @param[in] shapeIdA First shape in contact + * @param[in] shapeIdB Second shape in contact + */ + virtual void EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) {} + + /** + * @brief A method that is called for Box2D contact hit events (replaces PostSolve) + * @param[in] shapeIdA First shape in contact + * @param[in] shapeIdB Second shape in contact + * @param[in] point World-space contact point + * @param[in] normal Contact normal pointing from A to B + * @param[in] approachSpeed Approach speed of the collision + */ + virtual void OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, + float approachSpeed) {} + + /** + * @brief Flatland plugin destructor + */ + virtual ~FlatlandPlugin() = default; +}; +}; // namespace + +#endif // FLATLAND_SERVER_FLATLAND_PLUGIN_H diff --git a/flatland_server/include/flatland_server/geometry.h b/flatland_server/include/flatland_server/geometry.h index fd2ce689..93e56f9a 100644 --- a/flatland_server/include/flatland_server/geometry.h +++ b/flatland_server/include/flatland_server/geometry.h @@ -1,92 +1,92 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name geometry.h - * @brief Geometry functions - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_GEOMETRY_H -#define FLATLAND_SERVER_GEOMETRY_H - -#include - -namespace flatland_server { - -/** - * A struct to store precalculated 2D homogenous transformation values - */ -struct RotateTranslate { - double dx, dy; - double cos; - double sin; -}; - -class Geometry { - public: - /** - * @brief Creates a transform for given position and yaw delta - * @param[in] dx Delta x - * @param[in] dy Delta y - * @param[in] a Yaw - * @return The transformation - */ - static RotateTranslate CreateTransform(double dx, double dy, double a); - - /** - * @brief Transform a point for given transformation - * @param[in] in A point to transform - * @param[in] rt Defined transformation - * @return Transformed point - */ - static b2Vec2 Transform(const b2Vec2& in, const RotateTranslate& rt); - - /** - * @brief Inverse transform a point for given transformation - * @param[in] in A point to transform - * @param[in] rt Defined transformation - * @return Inverse transformed point - */ - static b2Vec2 InverseTransform(const b2Vec2& in, const RotateTranslate& rt); -}; - -}; // namespace flatland_server -#endif // FLATLAND_SERVER_GEOMETRY_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name geometry.h + * @brief Geometry functions + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_GEOMETRY_H +#define FLATLAND_SERVER_GEOMETRY_H + +#include + +namespace flatland_server { + +/** + * A struct to store precalculated 2D homogenous transformation values + */ +struct RotateTranslate { + double dx, dy; + double cos; + double sin; +}; + +class Geometry { + public: + /** + * @brief Creates a transform for given position and yaw delta + * @param[in] dx Delta x + * @param[in] dy Delta y + * @param[in] a Yaw + * @return The transformation + */ + static RotateTranslate CreateTransform(double dx, double dy, double a); + + /** + * @brief Transform a point for given transformation + * @param[in] in A point to transform + * @param[in] rt Defined transformation + * @return Transformed point + */ + static b2Vec2 Transform(const b2Vec2& in, const RotateTranslate& rt); + + /** + * @brief Inverse transform a point for given transformation + * @param[in] in A point to transform + * @param[in] rt Defined transformation + * @return Inverse transformed point + */ + static b2Vec2 InverseTransform(const b2Vec2& in, const RotateTranslate& rt); +}; + +}; // namespace flatland_server +#endif // FLATLAND_SERVER_GEOMETRY_H diff --git a/flatland_server/include/flatland_server/interactive_marker_manager.h b/flatland_server/include/flatland_server/interactive_marker_manager.h index e7f5679d..de931192 100644 --- a/flatland_server/include/flatland_server/interactive_marker_manager.h +++ b/flatland_server/include/flatland_server/interactive_marker_manager.h @@ -1,111 +1,111 @@ -#ifndef INTERACTIVEMARKERMANAGER_H -#define INTERACTIVEMARKERMANAGER_H - -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -class InteractiveMarkerManager { - public: - /** - * @brief Constructor for the interactive marker manager class - * @param[in] model_list_ptr Pointer to the list of models in the World class - * @param[in] plugin_manager_ptr Pointer to the plugin manager in the World - * class - */ - InteractiveMarkerManager(std::vector* model_list_ptr, - PluginManager* plugin_manager_ptr); - - /** - * @brief Destructor for the interactive marker manager class - */ - ~InteractiveMarkerManager(); - - /** - * @brief Add a new interactive marker when spawning a model - * @param[in] model_name Name of the model being spawned - * @param[in] pose Initial pose of the spawning model - * @param[in] body_markers Marker array corresponding to new model bodies - */ - void createInteractiveMarker( - const std::string& model_name, const Pose& pose, - const visualization_msgs::MarkerArray& body_markers); - - /** - * @brief Remove interactive marker corresponding to a given model when - * deleting it from the simulation - * @param[in] model_name Name of the model being deleted - */ - void deleteInteractiveMarker(const std::string& model_name); - - /** - * @brief Update the interactive marker poses after running - * physics update to synchronize the markers with the models - */ - void update(); - - bool isManipulating() { return manipulating_model_; } - - private: - interactive_markers::MenuHandler - menu_handler_; ///< Handler for the interactive marker context menus - boost::shared_ptr - interactive_marker_server_; ///< Interactive marker server - std::vector* - models_; ///< Pointer to the model list in the World class - PluginManager* - plugin_manager_; ///< Pointer to the plugin manager in the World class - bool manipulating_model_; ///< Boolean flag indicating if the user is - /// manipulating a model with its interactive marker - ros::WallTime pose_update_stamp_; ///< Timestamp of the last received pose - /// update feedback. Used to handle when the - /// interactive marker server stops - /// manipulating without triggering a - /// MOUSE_UP event. - - /** - * @brief Process interactive feedback on a MOUSE_UP event and use it - * to move the appropriate model to the new pose - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the new pose - */ - void processMouseUpFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - - /** - * @brief Process interactive feedback on a MOUSE_DOWN event and use it - * to set a flag indicating that the user is manipulating a model with - * its interactive marker - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the current pose. Not used in this callback - */ - void processMouseDownFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - - /** - * @brief Process interactive feedback on a POSE_UPDATE event and record - * the current wall time to detect a timeout in the update method - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the current pose. Not used in this method - */ - void processPoseUpdateFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - - /** - * @brief Process feedback from the context menu of the interactive marker to - * delete the appropriate model - * @param[in] feedback The feedback structure containing the name of the model - * to be deleted - */ - void deleteModelMenuCallback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); -}; -} - -#endif // INTERACTIVEMARKERMANAGER_H +#ifndef INTERACTIVEMARKERMANAGER_H +#define INTERACTIVEMARKERMANAGER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +class InteractiveMarkerManager { + public: + /** + * @brief Constructor for the interactive marker manager class + * @param[in] model_list_ptr Pointer to the list of models in the World class + * @param[in] plugin_manager_ptr Pointer to the plugin manager in the World + * class + */ + InteractiveMarkerManager(std::vector* model_list_ptr, + PluginManager* plugin_manager_ptr); + + /** + * @brief Destructor for the interactive marker manager class + */ + ~InteractiveMarkerManager(); + + /** + * @brief Add a new interactive marker when spawning a model + * @param[in] model_name Name of the model being spawned + * @param[in] pose Initial pose of the spawning model + * @param[in] body_markers Marker array corresponding to new model bodies + */ + void createInteractiveMarker( + const std::string& model_name, const Pose& pose, + const visualization_msgs::MarkerArray& body_markers); + + /** + * @brief Remove interactive marker corresponding to a given model when + * deleting it from the simulation + * @param[in] model_name Name of the model being deleted + */ + void deleteInteractiveMarker(const std::string& model_name); + + /** + * @brief Update the interactive marker poses after running + * physics update to synchronize the markers with the models + */ + void update(); + + bool isManipulating() { return manipulating_model_; } + + private: + interactive_markers::MenuHandler + menu_handler_; ///< Handler for the interactive marker context menus + boost::shared_ptr + interactive_marker_server_; ///< Interactive marker server + std::vector* + models_; ///< Pointer to the model list in the World class + PluginManager* + plugin_manager_; ///< Pointer to the plugin manager in the World class + bool manipulating_model_; ///< Boolean flag indicating if the user is + /// manipulating a model with its interactive marker + ros::WallTime pose_update_stamp_; ///< Timestamp of the last received pose + /// update feedback. Used to handle when the + /// interactive marker server stops + /// manipulating without triggering a + /// MOUSE_UP event. + + /** + * @brief Process interactive feedback on a MOUSE_UP event and use it + * to move the appropriate model to the new pose + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the new pose + */ + void processMouseUpFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + + /** + * @brief Process interactive feedback on a MOUSE_DOWN event and use it + * to set a flag indicating that the user is manipulating a model with + * its interactive marker + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the current pose. Not used in this callback + */ + void processMouseDownFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + + /** + * @brief Process interactive feedback on a POSE_UPDATE event and record + * the current wall time to detect a timeout in the update method + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the current pose. Not used in this method + */ + void processPoseUpdateFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); + + /** + * @brief Process feedback from the context menu of the interactive marker to + * delete the appropriate model + * @param[in] feedback The feedback structure containing the name of the model + * to be deleted + */ + void deleteModelMenuCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); +}; +} + +#endif // INTERACTIVEMARKERMANAGER_H diff --git a/flatland_server/include/flatland_server/joint.h b/flatland_server/include/flatland_server/joint.h index bdebb370..9163d354 100644 --- a/flatland_server/include/flatland_server/joint.h +++ b/flatland_server/include/flatland_server/joint.h @@ -1,181 +1,181 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name joint.h - * @brief Defines Joint - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_JOINT_H -#define FLATLAND_SERVER_JOINT_H - -#include -#include -#include -#include - -namespace flatland_server { - -class Model; - -/** - * This class defines a joint in the simulation world. It wraps around the Box2D - * physics joints providing extra data and useful methods - */ -class Joint { - public: - Model *model_; ///< Model the joint belongs to - std::string name_; ///< Name of the joint - b2World *physics_world_; ///< Box2D physics world - Color color_; ///< Color for visualization - b2Joint *physics_joint_; ///< Box2D physics joint - - /** - * @brief Constructor for the joint - * @param[in] physics_world Box2D physics world - * @param[in] model Model the joint belongs to - * @param[in] name Name of the joint - * @param[in] color Color to visualize the joint - * @param[in] joint_def Box2D joint definition - */ - Joint(b2World *physics_world, Model *model, const std::string &name, - const Color &color, const b2JointDef &joint_def); - ~Joint(); - - /// Disallow copying of joints, problematic for constructors and destructors - Joint(const Joint &) = delete; - Joint &operator=(const Joint &) = delete; - - /** - * @brief logs the debugging information for the joint - */ - void DebugOutput() const; - - /** - * @return Pointer to the model that contains the joint - */ - Model *GetModel(); - - /** - * @return Name of the the joint - */ - const std::string &GetName() const; - - /** - * @return Color of the joint for visualization - */ - const Color &GetColor() const; - - /** - * @return Color of the joint for visualization - */ - void SetColor(const Color &color); - - /** - * @return Get pointer to the Box2D physics joint - */ - b2Joint *GetPhysicsJoint(); - - /** - * @return Get pointer of the Box2D physics world - */ - b2World *GetphysicsWorld(); - - /** - * @brief Creates a joint for the given params, throws exceptions upon failure - * @param[in] physics_world Box2D physics world - * @param[in] model Model the joint belongs to - * @param[in] joint_reader YAML reader for node that contains joint - * information - * @return A new joint as defined by the input data - */ - static Joint *MakeJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader); - - /** - * @brief Creates a revolute joint for the given params, throws exceptions - * upon failure - * @param[in] physics_world Box2D physics world - * @param[in] model Model the joint belongs to - * @param[in] joint_reader YAML reader for node that contains joint - * information - * @param[in] name Name of the joint - * @param[in] color Color to visualize the joint - * @param[in] body_A pointer to the first body - * @param[in] anchor_B anchor point on the first body - * @param[in] body_B pointer to the second body - * @param[in] anchor_B anchor point on the second body - * @param[in] collide_connected Should two bodies connected by this joint - * collide - * @return A new revolute joint as defined by the input data - */ - static Joint *MakeRevoluteJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, - const std::string &name, const Color &color, - b2Body *body_A, b2Vec2 anchor_A, - b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected); - - /** - * @brief Creates a weld joint for the given params, throws exceptions upon - * failure - * @param[in] physics_world Box2D physics world - * @param[in] model Model the joint belongs to - * @param[in] joint_reader YAML reader for node that contains joint - * information - * @param[in] name Name of the joint - * @param[in] color Color to visualize the joint - * @param[in] body_A pointer to the first body - * @param[in] anchor_B anchor point on the first body - * @param[in] body_B pointer to the second body - * @param[in] anchor_B anchor point on the second body - * @param[in] collide_connected Should two bodies connected by this joint - * collide - * @return A new weld joint as defined by the input data - */ - static Joint *MakeWeldJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, const std::string &name, - const Color &color, b2Body *body_A, - b2Vec2 anchor_A, b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected); -}; -}; // namespace flatland_server -#endif // FLATLAND_MODEL_JOINT_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name joint.h + * @brief Defines Joint + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_JOINT_H +#define FLATLAND_SERVER_JOINT_H + +#include +#include +#include +#include + +namespace flatland_server { + +class Model; + +/** + * This class defines a joint in the simulation world. It wraps around the Box2D + * physics joints providing extra data and useful methods + */ +class Joint { + public: + Model *model_; ///< Model the joint belongs to + std::string name_; ///< Name of the joint + b2WorldId physics_world_; ///< Box2D v3 physics world ID + Color color_; ///< Color for visualization + b2JointId physics_joint_; ///< Box2D v3 physics joint ID + + /** + * @brief Constructor for the joint + * @param[in] physics_world Box2D physics world + * @param[in] model Model the joint belongs to + * @param[in] name Name of the joint + * @param[in] color Color to visualize the joint + * @param[in] joint_def Box2D joint definition + */ + Joint(b2WorldId physics_world, Model *model, const std::string &name, + const Color &color, b2JointId joint_id); + ~Joint(); + + /// Disallow copying of joints, problematic for constructors and destructors + Joint(const Joint &) = delete; + Joint &operator=(const Joint &) = delete; + + /** + * @brief logs the debugging information for the joint + */ + void DebugOutput() const; + + /** + * @return Pointer to the model that contains the joint + */ + Model *GetModel(); + + /** + * @return Name of the the joint + */ + const std::string &GetName() const; + + /** + * @return Color of the joint for visualization + */ + const Color &GetColor() const; + + /** + * @return Color of the joint for visualization + */ + void SetColor(const Color &color); + + /** + * @return Get pointer to the Box2D physics joint + */ + b2JointId GetPhysicsJoint(); + + /** + * @return Get the Box2D v3 physics world ID + */ + b2WorldId GetphysicsWorld(); + + /** + * @brief Creates a joint for the given params, throws exceptions upon failure + * @param[in] physics_world Box2D physics world + * @param[in] model Model the joint belongs to + * @param[in] joint_reader YAML reader for node that contains joint + * information + * @return A new joint as defined by the input data + */ + static Joint *MakeJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader); + + /** + * @brief Creates a revolute joint for the given params, throws exceptions + * upon failure + * @param[in] physics_world Box2D physics world + * @param[in] model Model the joint belongs to + * @param[in] joint_reader YAML reader for node that contains joint + * information + * @param[in] name Name of the joint + * @param[in] color Color to visualize the joint + * @param[in] body_A pointer to the first body + * @param[in] anchor_B anchor point on the first body + * @param[in] body_B pointer to the second body + * @param[in] anchor_B anchor point on the second body + * @param[in] collide_connected Should two bodies connected by this joint + * collide + * @return A new revolute joint as defined by the input data + */ + static Joint *MakeRevoluteJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader, + const std::string &name, const Color &color, + b2BodyId body_A, b2Vec2 anchor_A, + b2BodyId body_B, b2Vec2 anchor_B, + bool collide_connected); + + /** + * @brief Creates a weld joint for the given params, throws exceptions upon + * failure + * @param[in] physics_world Box2D physics world + * @param[in] model Model the joint belongs to + * @param[in] joint_reader YAML reader for node that contains joint + * information + * @param[in] name Name of the joint + * @param[in] color Color to visualize the joint + * @param[in] body_A pointer to the first body + * @param[in] anchor_B anchor point on the first body + * @param[in] body_B pointer to the second body + * @param[in] anchor_B anchor point on the second body + * @param[in] collide_connected Should two bodies connected by this joint + * collide + * @return A new weld joint as defined by the input data + */ + static Joint *MakeWeldJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader, const std::string &name, + const Color &color, b2BodyId body_A, + b2Vec2 anchor_A, b2BodyId body_B, b2Vec2 anchor_B, + bool collide_connected); +}; +}; // namespace flatland_server +#endif // FLATLAND_MODEL_JOINT_H diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h index 137ccee6..96ca3718 100644 --- a/flatland_server/include/flatland_server/layer.h +++ b/flatland_server/include/flatland_server/layer.h @@ -1,197 +1,197 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name layer.h - * @brief Defines flatland Layer - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_LAYER_H -#define FLATLAND_SERVER_LAYER_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a layer in the simulation world which simulates the - * environment in the world - */ -class Layer : public Entity { - public: - std::vector names_; ///< list of layer names - - Body *body_ = nullptr; - CollisionFilterRegistry *cfr_; ///< collision filter registry - std::string viz_name_; ///< for visualization - - /** - * @brief Constructor for the Layer class for initialization using a image - * map file - * @param[in] physics_world Pointer to the box2d physics world - * @param[in] cfr Collision filter registry - * @param[in] names A list of names for the layer, the first name is used - * for the name of the body - * @param[in] color Color in the form of r, g, b, a, used for visualization - * @param[in] origin Coordinate of the lower left corner of the image, in the - * form of x, y, theta - * @param[in] occupied_thresh Threshold indicating obstacle if above - * @param[in] bitmap Matrix containing the map image - * @param[in] resolution Resolution of the map image in meters per pixel - * @param[in] properties A YAML node containing properties for plugins to use - */ - Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, - double resolution, const YAML::Node &properties); - - /** - * @brief Constructor for the Layer class for initialization using line - * segments - * @param[in] physics_world Pointer to the box2d physics world - * @param[in] cfr Collision filter registry - * @param[in] names A list of names for the layer, the first name is used - * for the name of the body - * @param[in] color Color in the form of r, g, b, a, used for visualization - * @param[in] origin Coordinate of the lower left corner of the image, in the - * form of x, y, theta - * @param[in] line_segments List of line segments - * @param[in] scale Scale to apply to the line segment end points, works in - * the same way as resolution - * @param[in] properties A YAML node containing properties for plugins to use - */ - Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const std::vector &line_segments, - double scale, const YAML::Node &properties); - - /** - * @brief Constructor for the Layer class for initialization with no static - * map in it - * @param[in] physics_world Pointer to the box2d physics world - * @param[in] cfr Collision filter registry - * @param[in] names A list of names for the layer, the first name is used - * for the name of the body - * @param[in] properties A YAML node containing properties for plugins to use - */ - Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const YAML::Node &properties); - - /** - * @brief Destructor for the layer class - */ - ~Layer(); - - /** - * @return The list of names the layer has - */ - const std::vector &GetNames() const; - - /** - * @return The collision filter registrar - */ - const CollisionFilterRegistry *GetCfr() const; - - Body *GetBody(); - - /** - * @brief Return the type of entity - * @return type indicating it is a layer - */ - EntityType Type() const { return EntityType::LAYER; } - - /** - * @brief Load the map by extracting edges from images - * @param[in] bitmap OpenCV Image - * @param[in] occupied_thresh Threshold indicating obstacle if above - * @param[in] resolution Resolution of the map image in meters per pixel - */ - void LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, - double resolution); - - /** - * @brief Visualize layer for debugging purposes - */ - void DebugVisualize() const override; - - /** - * @brief log debug messages for the layer - */ - void DebugOutput() const override; - - /** - * @brief Read line segments from a file, each line of a file represents a - * line segment in the form of x1 y1 x2 y2 - * @param[in] file_path Path to the file - * @param[out] line_segments Line segments obtained from the file - */ - static void ReadLineSegmentsFile(const std::string &file_path, - std::vector &line_segments); - - /** - * @brief Factory method to instantiate a layer, throws exceptions upon - * failure - * @param[in] physics_world Pointer to the box2d physics world - * @param[in] cfr Collision filter registry - * @param[in] map_path Path to the file containing layer data - * @param[in] names A list of names for the layer, the first name is used - * for the name of the body. All names are registered in the CFR. Multiple - * names allow the physics body to be used as if there are multiple layers - * @param[in] color Color of the layer - * file, this is used to calculate the path to the layermap yaml file - * @param[in] properties A YAML node containing properties for plugins to use - * @return A new layer - */ - static Layer *MakeLayer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &map_path, - const std::vector &names, - const Color &color, const YAML::Node &properties); -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_WORLD_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name layer.h + * @brief Defines flatland Layer + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_LAYER_H +#define FLATLAND_SERVER_LAYER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a layer in the simulation world which simulates the + * environment in the world + */ +class Layer : public Entity { + public: + std::vector names_; ///< list of layer names + + Body *body_ = nullptr; + CollisionFilterRegistry *cfr_; ///< collision filter registry + std::string viz_name_; ///< for visualization + + /** + * @brief Constructor for the Layer class for initialization using a image + * map file + * @param[in] physics_world Pointer to the box2d physics world + * @param[in] cfr Collision filter registry + * @param[in] names A list of names for the layer, the first name is used + * for the name of the body + * @param[in] color Color in the form of r, g, b, a, used for visualization + * @param[in] origin Coordinate of the lower left corner of the image, in the + * form of x, y, theta + * @param[in] occupied_thresh Threshold indicating obstacle if above + * @param[in] bitmap Matrix containing the map image + * @param[in] resolution Resolution of the map image in meters per pixel + * @param[in] properties A YAML node containing properties for plugins to use + */ + Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, + double resolution, const YAML::Node &properties); + + /** + * @brief Constructor for the Layer class for initialization using line + * segments + * @param[in] physics_world Pointer to the box2d physics world + * @param[in] cfr Collision filter registry + * @param[in] names A list of names for the layer, the first name is used + * for the name of the body + * @param[in] color Color in the form of r, g, b, a, used for visualization + * @param[in] origin Coordinate of the lower left corner of the image, in the + * form of x, y, theta + * @param[in] line_segments List of line segments + * @param[in] scale Scale to apply to the line segment end points, works in + * the same way as resolution + * @param[in] properties A YAML node containing properties for plugins to use + */ + Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const Pose &origin, const std::vector &line_segments, + double scale, const YAML::Node &properties); + + /** + * @brief Constructor for the Layer class for initialization with no static + * map in it + * @param[in] physics_world Pointer to the box2d physics world + * @param[in] cfr Collision filter registry + * @param[in] names A list of names for the layer, the first name is used + * for the name of the body + * @param[in] properties A YAML node containing properties for plugins to use + */ + Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const YAML::Node &properties); + + /** + * @brief Destructor for the layer class + */ + ~Layer(); + + /** + * @return The list of names the layer has + */ + const std::vector &GetNames() const; + + /** + * @return The collision filter registrar + */ + const CollisionFilterRegistry *GetCfr() const; + + Body *GetBody(); + + /** + * @brief Return the type of entity + * @return type indicating it is a layer + */ + EntityType Type() const { return EntityType::LAYER; } + + /** + * @brief Load the map by extracting edges from images + * @param[in] bitmap OpenCV Image + * @param[in] occupied_thresh Threshold indicating obstacle if above + * @param[in] resolution Resolution of the map image in meters per pixel + */ + void LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, + double resolution); + + /** + * @brief Visualize layer for debugging purposes + */ + void DebugVisualize() const override; + + /** + * @brief log debug messages for the layer + */ + void DebugOutput() const override; + + /** + * @brief Read line segments from a file, each line of a file represents a + * line segment in the form of x1 y1 x2 y2 + * @param[in] file_path Path to the file + * @param[out] line_segments Line segments obtained from the file + */ + static void ReadLineSegmentsFile(const std::string &file_path, + std::vector &line_segments); + + /** + * @brief Factory method to instantiate a layer, throws exceptions upon + * failure + * @param[in] physics_world Pointer to the box2d physics world + * @param[in] cfr Collision filter registry + * @param[in] map_path Path to the file containing layer data + * @param[in] names A list of names for the layer, the first name is used + * for the name of the body. All names are registered in the CFR. Multiple + * names allow the physics body to be used as if there are multiple layers + * @param[in] color Color of the layer + * file, this is used to calculate the path to the layermap yaml file + * @param[in] properties A YAML node containing properties for plugins to use + * @return A new layer + */ + static Layer *MakeLayer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &map_path, + const std::vector &names, + const Color &color, const YAML::Node &properties); +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/message_server.h b/flatland_server/include/flatland_server/message_server.h new file mode 100644 index 00000000..a6f394a5 --- /dev/null +++ b/flatland_server/include/flatland_server/message_server.h @@ -0,0 +1,113 @@ +#ifndef FLATLAND_SERVER_MESSAGE_SERVER_H +#define FLATLAND_SERVER_MESSAGE_SERVER_H + +#include +#include + +#include + +namespace flatland_server { + +template +class Subscriber; +template +class Publisher; +class MessageServer; +class MessageTopicBase {}; + +template +class MessageTopic : public MessageTopicBase { + public: + std::vector> subscribers_; +}; + +template +class Subscriber { + public: + Subscriber() = default; + + explicit Subscriber(MessageTopic* topic, + const std::function& callback_function) + : topic_(topic), callback_function_(callback_function) {} + + private: + MessageTopic* topic_; + std::function callback_function_; + + friend class Publisher; +}; + +template +class Publisher { + public: + Publisher() = default; + explicit Publisher(MessageTopic* topic) : topic_(topic) {} + + void publish(const T&); + + private: + MessageTopic* topic_; +}; + +class MessageServer { + private: + std::unordered_map> topics_; + + template + MessageTopic* get_message_topic(const std::string& name); + + public: + template + Subscriber subscribe( + const std::string& name, + const std::function& callback_function); + + template + Publisher advertise(const std::string& name); +}; + +/// +/// Start of Implementation section +/// + +template +MessageTopic* MessageServer::get_message_topic(const std::string& name) { + auto msg_topic = topics_.find(name); + if (msg_topic == topics_.end()) { + msg_topic = topics_ + .emplace(name, std::unique_ptr>( + new flatland_server::MessageTopic())) + .first; + } + return static_cast*>((msg_topic->second).get()); +} + +template +Subscriber MessageServer::subscribe( + const std::string& name, + const std::function& callback_function) { + flatland_server::MessageTopic* topic = get_message_topic(name); + + // TODO - Make it so that when the copied subscriber gets deleted, it will be + // removed from the topic list + // Not bothering to fix it now because, currently deletion of subscribers has + // not been fully implemented + topic->subscribers_.emplace_back(topic, callback_function); + return topic->subscribers_.back(); +} + +template +Publisher MessageServer::advertise(const std::string& name) { + flatland_server::MessageTopic* topic = get_message_topic(name); + return Publisher(topic); +} + +template +void Publisher::publish(const T& t) { + for (const Subscriber& subscriber : topic_->subscribers_) { + subscriber.callback_function_(t); + } +} +} + +#endif // FLATLAND_SERVER_MESSAGE_H diff --git a/flatland_server/include/flatland_server/model.h b/flatland_server/include/flatland_server/model.h index 5f115fff..7663643d 100644 --- a/flatland_server/include/flatland_server/model.h +++ b/flatland_server/include/flatland_server/model.h @@ -1,198 +1,212 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model.h - * @brief Defines flatland Model - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_MODEL_H -#define FLATLAND_SERVER_MODEL_H - -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -class ModelBody; -class Joint; - -/** - * This class defines a Model. It can be used to repsent any object in the - * environment such robots, chairs, deskes etc. - */ -class Model : public Entity { - public: - std::string namespace_; ///< namespace of the model - std::vector bodies_; ///< list of bodies in the model - std::vector joints_; ///< list of joints in the model - YamlReader plugins_reader_; ///< for storing plugins when paring YAML - CollisionFilterRegistry *cfr_; ///< Collision filter registry - std::string viz_name_; ///< used for visualization - - /** - * @brief Constructor for the model - * @param[in] physics_world Box2D physics world - * @param[in] cfr Collision filter registry - * @param[in] name Name of the model - */ - Model(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &ns, const std::string &name); - - /** - * @brief Destructor for the layer class - */ - ~Model(); - - /** - * @brief Return the type of entity - * @return Model type - */ - EntityType Type() const { return EntityType::MODEL; } - - /** - * @brief load bodies to this model, throws exceptions upon failure - * @param[in] bodies_reader YAML reader for node containing the list of bodies - */ - void LoadBodies(YamlReader &bodies_reader); - - /** - * @brief load joints to this model, throws exceptions upon failure - * @param[in] joints_reader YAML reader for node containing the list of joints - */ - void LoadJoints(YamlReader &joints_reader); - - /** - * @brief Get a body in the model using its name - * @param[in] name Name of the body - * @return pointer to the body, nullptr indicates body cannot be found - */ - ModelBody *GetBody(const std::string &name); - - /** - * @brief Get a body in the model using its name - * @param[in] name Name of the joint - * @return pointer to the joint, nullptr if the joint does not exist - */ - Joint *GetJoint(const std::string &name); - - /** - * @return List of bodies the model has - */ - const std::vector &GetBodies(); - - /** - * @return List of joints the model has - */ - const std::vector &GetJoints(); - - /** - * @return The namespace of the model - */ - const std::string &GetNameSpace() const; - - /** - * @return prepend the tf with namespace_ - */ - std::string NameSpaceTF(const std::string &frame_id) const; - - /** - * @return prepend the topic with namespace/ - */ - std::string NameSpaceTopic(const std::string &topic) const; - - /** - * @return The name of the model - */ - const std::string &GetName() const; - - /** - * @return The collision filter registrar - */ - const CollisionFilterRegistry *GetCfr() const; - - /** - * @brief Publish debug visualizations for model - */ - void DebugVisualize() const override; - - /** - * @brief log debug messages for the layer - */ - void DebugOutput() const override; - - /** - * @brief Dump box2d data for debugging - */ - void DumpBox2D() const; - - /** - * @brief transform all bodies in the model - * @param[in] pose_delta dx, dy, dyaw - */ - void TransformAll(const Pose &pose_delta); - - /** - * @brief Explicitly set the model pose in world coordinates - * @param[in] pose world x, world y, world yaw - */ - void SetPose(const Pose &pose); - - /** - * @brief Create a model, throws exceptions upon failure - * @param[in] physics_world Box2D physics world - * @param[in] cfr Collision filter registry - * @param[in] model_yaml_path Absolute path to the model yaml file - * @param[in] ns Namespace of the robot - * @param[in] name Name of the model - * @return A new model - */ - static Model *MakeModel(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &model_yaml_path, - const std::string &ns, const std::string &name); -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_MODEL_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model.h + * @brief Defines flatland Model + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_MODEL_H +#define FLATLAND_SERVER_MODEL_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +class ModelBody; +class Joint; +class World; + +/** + * This class defines a Model. It can be used to repsent any object in the + * environment such robots, chairs, deskes etc. + */ +class Model : public Entity { + private: + World *world_; ///< pointer to parent world (may be nullptr for legacy construction) + + public: + std::string namespace_; ///< namespace of the model + std::vector bodies_; ///< list of bodies in the model + std::vector joints_; ///< list of joints in the model + YamlReader plugins_reader_; ///< for storing plugins when paring YAML + CollisionFilterRegistry *cfr_; ///< Collision filter registry + std::string viz_name_; ///< used for visualization + + /** + * @brief Constructor for the model + * @param[in] physics_world Box2D physics world + * @param[in] cfr Collision filter registry + * @param[in] name Name of the model + */ + Model(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &ns, const std::string &name); + Model(World *world, b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &ns, const std::string &name); + + /** + * @brief Destructor for the layer class + */ + ~Model(); + + /** + * @brief Return the type of entity + * @return Model type + */ + EntityType Type() const { return EntityType::MODEL; } + + /** + * @brief load bodies to this model, throws exceptions upon failure + * @param[in] bodies_reader YAML reader for node containing the list of bodies + */ + void LoadBodies(YamlReader &bodies_reader); + + /** + * @brief load joints to this model, throws exceptions upon failure + * @param[in] joints_reader YAML reader for node containing the list of joints + */ + void LoadJoints(YamlReader &joints_reader); + + /** + * @brief Get a body in the model using its name + * @param[in] name Name of the body + * @return pointer to the body, nullptr indicates body cannot be found + */ + World &GetWorld() const; + MessageServer &GetMessageServer() const; + + ModelBody *GetBody(const std::string &name); + + /** + * @brief Get a body in the model using its name + * @param[in] name Name of the joint + * @return pointer to the joint, nullptr if the joint does not exist + */ + Joint *GetJoint(const std::string &name); + + /** + * @return List of bodies the model has + */ + const std::vector &GetBodies(); + + /** + * @return List of joints the model has + */ + const std::vector &GetJoints(); + + /** + * @return The namespace of the model + */ + const std::string &GetNameSpace() const; + + /** + * @return prepend the tf with namespace_ + */ + std::string NameSpaceTF(const std::string &frame_id) const; + + /** + * @return prepend the topic with namespace/ + */ + std::string NameSpaceTopic(const std::string &topic) const; + + /** + * @return The name of the model + */ + const std::string &GetName() const; + + /** + * @return The collision filter registrar + */ + const CollisionFilterRegistry *GetCfr() const; + + /** + * @brief Publish debug visualizations for model + */ + void DebugVisualize() const override; + + /** + * @brief log debug messages for the layer + */ + void DebugOutput() const override; + + /** + * @brief Dump box2d data for debugging + */ + void DumpBox2D() const; + + /** + * @brief transform all bodies in the model + * @param[in] pose_delta dx, dy, dyaw + */ + void TransformAll(const Pose &pose_delta); + + /** + * @brief Explicitly set the model pose in world coordinates + * @param[in] pose world x, world y, world yaw + */ + void SetPose(const Pose &pose); + + /** + * @brief Create a model, throws exceptions upon failure + * @param[in] physics_world Box2D physics world + * @param[in] cfr Collision filter registry + * @param[in] model_yaml_path Absolute path to the model yaml file + * @param[in] ns Namespace of the robot + * @param[in] name Name of the model + * @return A new model + */ + static Model *MakeModel(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &model_yaml_path, + const std::string &ns, const std::string &name); + static Model *MakeModel(World *world, b2WorldId physics_world, + CollisionFilterRegistry *cfr, + const std::string &model_yaml_path, + const std::string &ns, const std::string &name); +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_MODEL_H diff --git a/flatland_server/include/flatland_server/model_body.h b/flatland_server/include/flatland_server/model_body.h index 94607a67..287a34a2 100644 --- a/flatland_server/include/flatland_server/model_body.h +++ b/flatland_server/include/flatland_server/model_body.h @@ -1,135 +1,135 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_body.h - * @brief Defines Model Body - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_MODEL_BODY_H -#define FLATLAND_SERVER_MODEL_BODY_H - -#include -#include -#include -#include -#include - -namespace flatland_server { - -class Model; - -/** - * This class defines a model body which is a body that is always used together - * with a model. It contains members and useful methods that is specfic for a - * body in a model - */ -class ModelBody : public Body { - public: - CollisionFilterRegistry *cfr_; ///< collision filter registry - - /** - * @brief Constructor for the Model Body - * @param[in] physics_world Box2D physics world - * @param[in] cfr Collision filter registry - * @param[in] model Model the body belongs to - * @param[in] name Name of the body - * @param[in] color Color of the body for visualization - * @param[in] pose The pose to place the body at - * @param[in] body_type Type of Box2D body, either dynamic, static, or - * kinematic - * @param[in] properties per-object YAML properties for plugins to reference - * @param[in] linear_damping Box2D body linear damping - * @param[in] angular_damping Box2D body angular damping - */ - ModelBody(b2World *physics_world, CollisionFilterRegistry *cfr, Model *model, - const std::string &name, const Color &color, const Pose &pose, - b2BodyType body_type, const YAML::Node &properties, - double linear_damping, double angular_damping); - - /** - * @return The collision filter registry - */ - const CollisionFilterRegistry *GetCfr() const; - - /** - * @brief Load footprints (Box2D fixtures) into the body - * @param[in] footprints_reader YAML reader for node containing the footprints - * parameters - */ - void LoadFootprints(YamlReader &footprints_reader); - - /** - * @brief Configures the common properties of footprints - * @param[in] footprint_reader YAML reader for node containing the footprint - * parameters - * @param[out] fixture_def Box2D fixture definition - */ - void ConfigFootprintDef(YamlReader &footprint_reader, - b2FixtureDef &fixture_def); - - /** - * @brief Loads a circle footprint - * @param[in] footprint_reader YAML reader for node containing the footprint - * parameters - */ - void LoadCircleFootprint(YamlReader &footprint_reader); - - /** - * @brief Loads a circle footprint - * @param[in] footprint_reader YAML reader for node containing the footprint - * parameters - */ - void LoadPolygonFootprint(YamlReader &footprint_reader); - - /** - * @brief Factory method to create a model body - * @param[in] physics_world Box2D physics world - * @param[in] cfr Collision filter registry - * @param[in] model The model this model body belongs to - * @param[in] body_node YAML reader for node containing the body parameters - */ - static ModelBody *MakeBody(b2World *physics_world, - CollisionFilterRegistry *cfr, Model *model, - YamlReader &body_node); -}; -}; // namespace flatland_server -#endif // FLATLAND_MODEL_BODY_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_body.h + * @brief Defines Model Body + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_MODEL_BODY_H +#define FLATLAND_SERVER_MODEL_BODY_H + +#include +#include +#include +#include +#include + +namespace flatland_server { + +class Model; + +/** + * This class defines a model body which is a body that is always used together + * with a model. It contains members and useful methods that is specfic for a + * body in a model + */ +class ModelBody : public Body { + public: + CollisionFilterRegistry *cfr_; ///< collision filter registry + + /** + * @brief Constructor for the Model Body + * @param[in] physics_world Box2D physics world + * @param[in] cfr Collision filter registry + * @param[in] model Model the body belongs to + * @param[in] name Name of the body + * @param[in] color Color of the body for visualization + * @param[in] pose The pose to place the body at + * @param[in] body_type Type of Box2D body, either dynamic, static, or + * kinematic + * @param[in] properties per-object YAML properties for plugins to reference + * @param[in] linear_damping Box2D body linear damping + * @param[in] angular_damping Box2D body angular damping + */ + ModelBody(b2WorldId physics_world, CollisionFilterRegistry *cfr, Model *model, + const std::string &name, const Color &color, const Pose &pose, + b2BodyType body_type, const YAML::Node &properties, + double linear_damping, double angular_damping); + + /** + * @return The collision filter registry + */ + const CollisionFilterRegistry *GetCfr() const; + + /** + * @brief Load footprints (Box2D fixtures) into the body + * @param[in] footprints_reader YAML reader for node containing the footprints + * parameters + */ + void LoadFootprints(YamlReader &footprints_reader); + + /** + * @brief Configures the common properties of footprints + * @param[in] footprint_reader YAML reader for node containing the footprint + * parameters + * @param[out] fixture_def Box2D fixture definition + */ + void ConfigFootprintDef(YamlReader &footprint_reader, + b2ShapeDef &shape_def); + + /** + * @brief Loads a circle footprint + * @param[in] footprint_reader YAML reader for node containing the footprint + * parameters + */ + void LoadCircleFootprint(YamlReader &footprint_reader); + + /** + * @brief Loads a circle footprint + * @param[in] footprint_reader YAML reader for node containing the footprint + * parameters + */ + void LoadPolygonFootprint(YamlReader &footprint_reader); + + /** + * @brief Factory method to create a model body + * @param[in] physics_world Box2D physics world + * @param[in] cfr Collision filter registry + * @param[in] model The model this model body belongs to + * @param[in] body_node YAML reader for node containing the body parameters + */ + static ModelBody *MakeBody(b2WorldId physics_world, + CollisionFilterRegistry *cfr, Model *model, + YamlReader &body_node); +}; +}; // namespace flatland_server +#endif // FLATLAND_MODEL_BODY_H diff --git a/flatland_server/include/flatland_server/model_plugin.h b/flatland_server/include/flatland_server/model_plugin.h index c1a88680..39f340ec 100644 --- a/flatland_server/include/flatland_server/model_plugin.h +++ b/flatland_server/include/flatland_server/model_plugin.h @@ -1,117 +1,118 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_plugin.h - * @brief Interface for ModelPlugin pluginlib plugins - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_MODEL_PLUGIN_H -#define FLATLAND_SERVER_MODEL_PLUGIN_H - -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a model plugin. All implemented model plugins will inherit - * from it A model plugin is a plugin that is directly tied to a single model in - * the world - */ -class ModelPlugin : public FlatlandPlugin { - private: - Model *model_; ///< model this plugin is tied to - - public: - ros::NodeHandle nh_; ///< ROS node handle - - /** - * @brief Get model - */ - Model *GetModel(); - - /** - * @brief The method to initialize the ModelPlugin, required since Pluginlib - * require the class to have a default constructor - * @param[in] type Type of the plugin - * @param[in] name Name of the plugin - * @param[in] model The model associated with this model plugin - * @param[in] config The plugin YAML node - */ - void Initialize(const std::string &type, const std::string &name, - Model *model, const YAML::Node &config); - - /** - * @brief Helper function check if this model is part of the contact, and - * extracts all the useful information - * @param[in] contact Box2D contact - * @param[out] entity The entity that collided with this model - * @param[out] this_fixture The fixture from this model involved in the - * collision - * @param[out] other_fixture The fixture from the other entity involved in the - * collision - * @return True or false depending on if this model is involved. If false - * is returned, none of the entity, this_fixture, other_fixture pointers will - * be populated - */ - bool FilterContact(b2Contact *contact, Entity *&entity, - b2Fixture *&this_fixture, b2Fixture *&other_fixture); - - /** - * @brief Helper function check if this model is part of the contact - * @param[in] contact Box2D contact - * @return True or false depending on if this model is involved - */ - bool FilterContact(b2Contact *contact); - - protected: - /** - * @brief Model plugin default constructor - */ - ModelPlugin() = default; -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_MODEL_PLUGIN_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_plugin.h + * @brief Interface for ModelPlugin pluginlib plugins + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_MODEL_PLUGIN_H +#define FLATLAND_SERVER_MODEL_PLUGIN_H + +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a model plugin. All implemented model plugins will inherit + * from it A model plugin is a plugin that is directly tied to a single model in + * the world + */ +class ModelPlugin : public FlatlandPlugin { + private: + Model *model_; ///< model this plugin is tied to + + public: + ros::NodeHandle nh_; ///< ROS node handle + + /** + * @brief Get model + */ + Model *GetModel(); + + /** + * @brief The method to initialize the ModelPlugin, required since Pluginlib + * require the class to have a default constructor + * @param[in] type Type of the plugin + * @param[in] name Name of the plugin + * @param[in] model The model associated with this model plugin + * @param[in] config The plugin YAML node + */ + void Initialize(const std::string &type, const std::string &name, + Model *model, const YAML::Node &config); + + /** + * @brief Helper function check if this model is part of the contact, and + * extracts all the useful information + * @param[in] contact Box2D contact + * @param[out] entity The entity that collided with this model + * @param[out] this_fixture The fixture from this model involved in the + * collision + * @param[out] other_fixture The fixture from the other entity involved in the + * collision + * @return True or false depending on if this model is involved. If false + * is returned, none of the entity, this_body, other_body pointers will + * be populated + */ + bool FilterContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB, Entity *&entity, + b2BodyId &this_body, b2BodyId &other_body); + + /** + * @brief Helper function check if this model is part of the contact + * @param[in] shapeIdA First shape in contact + * @param[in] shapeIdB Second shape in contact + * @return True or false depending on if this model is involved + */ + bool FilterContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB); + + protected: + /** + * @brief Model plugin default constructor + */ + ModelPlugin() = default; +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_MODEL_PLUGIN_H diff --git a/flatland_server/include/flatland_server/plugin_manager.h b/flatland_server/include/flatland_server/plugin_manager.h index 08512a28..3cbe533f 100644 --- a/flatland_server/include/flatland_server/plugin_manager.h +++ b/flatland_server/include/flatland_server/plugin_manager.h @@ -1,143 +1,142 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name plugin_manager.h - * @brief Definition for plugin manager - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_PLUGIN_MANAGER_H -#define FLATLAND_PLUGIN_MANAGER_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -// forward declaration -class World; - -class PluginManager { - public: - std::vector> model_plugins_; - pluginlib::ClassLoader *model_plugin_loader_; - - std::vector> world_plugins_; - pluginlib::ClassLoader *world_plugin_loader_; - /** - * @brief Plugin manager constructor - */ - PluginManager(); - - /** - * @brief Plugin manager destructor - */ - ~PluginManager(); - - /** - * @brief This method is called before the Box2D physics step - * @param[in] timekeeper provide time related information - */ - void BeforePhysicsStep(const Timekeeper &timekeeper); - - /** - * @brief This method is called after the Box2D physics step - * @param[in] timekeeper provide time related information - */ - void AfterPhysicsStep(const Timekeeper &timekeeper); - - /** - * @brief This method removes all model plugins associated with a given mode - * @param[in] The model plugins is associated to - */ - void DeleteModelPlugin(Model *model); - - /** - * @brief Load model plugins - * @param[in] model The model that this plugin is tied to - * @param[in] plugin_reader The YAML reader with node containing the plugin - * parameter - */ - void LoadModelPlugin(Model *model, YamlReader &plugin_reader); - - /* - * @brief load world plugins - * @param[in] world, the world that thsi plugin is tied to - * @param[in] plugin_reader, the YAML reader with node containing the plugin - * @param[in] world_config, the yaml reader of world.yaml - */ - void LoadWorldPlugin(World *world, YamlReader &plugin_reader, - YamlReader &world_config); - - /** - * @brief Method called for a box2D begin contact - * @param[in] contact Box2D contact information - */ - void BeginContact(b2Contact *contact); - - /** - * @brief Method called for a box2D end contact - * @param[in] contact Box2D contact information - */ - void EndContact(b2Contact *contact); - - /** - * @brief Method called for Box2D presolve - * @param[in] contact Box2D contact information - * @param[in] oldManifold The manifold from the previous timestep - */ - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold); - - /** - * @brief Method called for Box2D Postsolve - * @param[in] contact Box2D contact information - * @param[in] impulse The calculated impulse from the collision resolute - */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse); -}; -}; // namespace flatland_server -#endif // FLATLAND_PLUGIN_MANAGER_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name plugin_manager.h + * @brief Definition for plugin manager + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_PLUGIN_MANAGER_H +#define FLATLAND_PLUGIN_MANAGER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +// forward declaration +class World; + +class PluginManager { + public: + std::vector> model_plugins_; + pluginlib::ClassLoader *model_plugin_loader_; + + std::vector> world_plugins_; + pluginlib::ClassLoader *world_plugin_loader_; + /** + * @brief Plugin manager constructor + */ + PluginManager(); + + /** + * @brief Plugin manager destructor + */ + ~PluginManager(); + + /** + * @brief This method is called before the Box2D physics step + * @param[in] timekeeper provide time related information + */ + void BeforePhysicsStep(const Timekeeper &timekeeper); + + /** + * @brief This method is called after the Box2D physics step + * @param[in] timekeeper provide time related information + */ + void AfterPhysicsStep(const Timekeeper &timekeeper); + + /** + * @brief This method removes all model plugins associated with a given mode + * @param[in] The model plugins is associated to + */ + void DeleteModelPlugin(Model *model); + + /** + * @brief Load model plugins + * @param[in] model The model that this plugin is tied to + * @param[in] plugin_reader The YAML reader with node containing the plugin + * parameter + */ + void LoadModelPlugin(Model *model, YamlReader &plugin_reader); + + /* + * @brief load world plugins + * @param[in] world, the world that thsi plugin is tied to + * @param[in] plugin_reader, the YAML reader with node containing the plugin + * @param[in] world_config, the yaml reader of world.yaml + */ + void LoadWorldPlugin(World *world, YamlReader &plugin_reader, + YamlReader &world_config); + + /** + * @brief Method called for a box2D begin contact + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + */ + void BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB); + + /** + * @brief Method called for a box2D end contact + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + */ + void EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB); + + /** + * @brief Method called for Box2D contact hit events + * @param[in] shapeIdA First shape + * @param[in] shapeIdB Second shape + * @param[in] point World-space contact point + * @param[in] normal Contact normal + * @param[in] approachSpeed Approach speed + */ + void OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, float approachSpeed); +}; +}; // namespace flatland_server +#endif // FLATLAND_PLUGIN_MANAGER_H diff --git a/flatland_server/include/flatland_server/profiler.h b/flatland_server/include/flatland_server/profiler.h new file mode 100644 index 00000000..c0231369 --- /dev/null +++ b/flatland_server/include/flatland_server/profiler.h @@ -0,0 +1,94 @@ +#ifndef FLATLAND_PROFILER_H +#define FLATLAND_PROFILER_H + +#define PROFILER_OUTPUT_PATH "/tmp/flatland_profile_output.log" + +// Define the preprocessor macro PROFILE_ON to activate profiling +#ifdef PROFILER_ON +#define START_PROFILE(timekeeper, name) timekeeper.profiler_.get(name).start() +#define END_PROFILE(timekeeper, name) timekeeper.profiler_.get(name).end() +#define PRINT_ALL_PROFILES(timekeeper) timekeeper.profiler_.print() +#else +#define START_PROFILE(timekeeper, name) +#define END_PROFILE(timekeeper, name) +#define PRINT_ALL_PROFILES(timekeeper) +#endif + +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +class Profile { + public: + Profile() + : lapse_count_(0), + started_(false), + start_time_(std::chrono::high_resolution_clock::now()), + total_duration_(std::chrono::high_resolution_clock::duration::zero()) {} + + void start() { + started_ = true; + start_time_ = std::chrono::high_resolution_clock::now(); + } + + void end() { + assert(started_); + lapse_count_++; + total_duration_ += std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start_time_); + } + + int getLapseCount() const { return lapse_count_; } + + int64_t getTotalDuration() const { + return std::chrono::duration_cast( + total_duration_) + .count(); + } + + private: + int lapse_count_; + bool started_; + std::chrono::high_resolution_clock::time_point start_time_; + std::chrono::high_resolution_clock::duration total_duration_; +}; + +class Profiler { + public: + Profile& get(const std::string& profile_name) { + return profiles_[profile_name]; + } + + void print() { + std::ofstream out(PROFILER_OUTPUT_PATH); + + out << std::left << std::setw(60) << "Profile" << std::left << std::setw(20) + << "Lapse Count" << std::left << std::setw(20) << "Total Duration (us)" + << std::left << std::setw(20) << "Average Duration (us)" << std::endl; + + for (const auto& prof : profiles_) { + out << std::left << std::setw(60) << prof.first << std::left + << std::setw(20) << prof.second.getLapseCount() << std::left + << std::setw(20) << prof.second.getTotalDuration() << std::left + << std::setw(20) << std::setprecision(10) + << ((prof.second.getLapseCount() == 0) + ? 0 + : (prof.second.getTotalDuration() / + prof.second.getLapseCount())) + << std::endl; + } + out.close(); + } + + private: + std::unordered_map profiles_; +}; + +} // namespace flatland_server + +#endif // FLATLAND_PROFILER_H diff --git a/flatland_server/include/flatland_server/service_manager.h b/flatland_server/include/flatland_server/service_manager.h index 0ae41f11..75a774b2 100644 --- a/flatland_server/include/flatland_server/service_manager.h +++ b/flatland_server/include/flatland_server/service_manager.h @@ -1,129 +1,129 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_spawner.h - * @brief Definition for model spawner - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -#ifndef FLATLAND_PLUGIN_SERVICE_MANAGER_H -#define FLATLAND_PLUGIN_SERVICE_MANAGER_H - -namespace flatland_server { - -class SimulationManager; - -/** - * This class contains a collection of ROS services that the user may use - * to work with the simulation - */ -class ServiceManager { - public: - World *world_; ///< aaa handle to the simulation world - SimulationManager *sim_man_; ///< a handle to the simulation manager - - ros::ServiceServer spawn_model_service_; ///< service for spawning models - ros::ServiceServer delete_model_service_; ///< service for deleting models - ros::ServiceServer move_model_service_; ///< service for moving models - ros::ServiceServer pause_service_; ///< service for pausing the simulation - ros::ServiceServer resume_service_; ///< service for resuming the simulation - ros::ServiceServer toggle_pause_service_; ///< service for toggling the - /// pause state of the simulation - - /** - * @brief Service manager constructor - * @param[in] sim_man A handle to the simulation manager - * @param[in] world A handle to the simulation world - */ - ServiceManager(SimulationManager *sim_man, World *world); - - /** - * @brief Callback for the spawn model service - * @param[in] request Contains the request data for the service - * @param[in/out] response Contains the response for the service - */ - bool SpawnModel(flatland_msgs::SpawnModel::Request &request, - flatland_msgs::SpawnModel::Response &response); - - /** - * @brief Callback for the delete model service - * @param[in] request Contains the request data for the service - * @param[in/out] response Contains the response for the service - */ - bool DeleteModel(flatland_msgs::DeleteModel::Request &request, - flatland_msgs::DeleteModel::Response &response); - - /** - * @brief Callback for the move model service - * @param[in] request Contains the request data for the service - * @param[in/out] response Contains the response for the service - */ - bool MoveModel(flatland_msgs::MoveModel::Request &request, - flatland_msgs::MoveModel::Response &response); - - /** - * @brief Callback for the pause service - */ - bool Pause(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response); - - /** - * @brief Callback for the resume service - */ - bool Resume(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response); - - /** - * @brief Callback for the pause toggle service - */ - bool TogglePause(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response); -}; -}; -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_spawner.h + * @brief Definition for model spawner + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#ifndef FLATLAND_PLUGIN_SERVICE_MANAGER_H +#define FLATLAND_PLUGIN_SERVICE_MANAGER_H + +namespace flatland_server { + +class SimulationManager; + +/** + * This class contains a collection of ROS services that the user may use + * to work with the simulation + */ +class ServiceManager { + public: + World *world_; ///< aaa handle to the simulation world + SimulationManager *sim_man_; ///< a handle to the simulation manager + + ros::ServiceServer spawn_model_service_; ///< service for spawning models + ros::ServiceServer delete_model_service_; ///< service for deleting models + ros::ServiceServer move_model_service_; ///< service for moving models + ros::ServiceServer pause_service_; ///< service for pausing the simulation + ros::ServiceServer resume_service_; ///< service for resuming the simulation + ros::ServiceServer toggle_pause_service_; ///< service for toggling the + /// pause state of the simulation + + /** + * @brief Service manager constructor + * @param[in] sim_man A handle to the simulation manager + * @param[in] world A handle to the simulation world + */ + ServiceManager(SimulationManager *sim_man, World *world); + + /** + * @brief Callback for the spawn model service + * @param[in] request Contains the request data for the service + * @param[in/out] response Contains the response for the service + */ + bool SpawnModel(flatland_msgs::SpawnModel::Request &request, + flatland_msgs::SpawnModel::Response &response); + + /** + * @brief Callback for the delete model service + * @param[in] request Contains the request data for the service + * @param[in/out] response Contains the response for the service + */ + bool DeleteModel(flatland_msgs::DeleteModel::Request &request, + flatland_msgs::DeleteModel::Response &response); + + /** + * @brief Callback for the move model service + * @param[in] request Contains the request data for the service + * @param[in/out] response Contains the response for the service + */ + bool MoveModel(flatland_msgs::MoveModel::Request &request, + flatland_msgs::MoveModel::Response &response); + + /** + * @brief Callback for the pause service + */ + bool Pause(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response); + + /** + * @brief Callback for the resume service + */ + bool Resume(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response); + + /** + * @brief Callback for the pause toggle service + */ + bool TogglePause(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response); +}; +}; +#endif diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h index b85bac0b..493c4005 100644 --- a/flatland_server/include/flatland_server/simulation_manager.h +++ b/flatland_server/include/flatland_server/simulation_manager.h @@ -1,91 +1,100 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name simulation_manager.h - * @brief Simulation manager class header definition - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_SIMULATION_MANAGER_H -#define FLATLAND_SERVER_SIMULATION_MANAGER_H - -#include -#include -#include -#include -#include - -namespace flatland_server { - -class SimulationManager { - public: - bool run_simulator_; ///< While true, keep running the sim loop - World *world_; ///< Simulation world - double update_rate_; ///< sim loop rate - double step_size_; ///< step size - bool show_viz_; ///< flag to determine if to show visualization - double viz_pub_rate_; ///< rate to publish visualization - std::string world_yaml_file_; ///< path to the world file - - /** - * @name Simulation Manager constructor - * @param[in] world_file The path to the world.yaml file we wish to load - * @param[in] update_rate Simulator loop rate - * @param[in] step_size Time to step each iteration - * @param[in] show_viz if to show visualization - * @param[in] viz_pub_rate rate to publish visualization - * behaving ones - */ - SimulationManager(std::string world_yaml_file, double update_rate, - double step_size, bool show_viz, double viz_pub_rate); - - /** - * This method contains the loop that runs the simulation - */ - void Main(); - - /** - * Kill the world - */ - void Shutdown(); -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_SIMULATION_MANAGER_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name simulation_manager.h + * @brief Simulation manager class header definition + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_SIMULATION_MANAGER_H +#define FLATLAND_SERVER_SIMULATION_MANAGER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +class ServiceManager; +class SimulationManager { + public: + bool run_simulator_; ///< While true, keep running the sim loop + World *world_; ///< Simulation world + double update_rate_; ///< sim loop rate + double step_size_; ///< step size + bool show_viz_; ///< flag to determine if to show visualization + double viz_pub_rate_; ///< rate to publish visualization + std::string world_yaml_file_; ///< path to the world file + std::string models_path_; ///< path to models directory + std::string world_plugins_path_; ///< path to world plugins yaml + + std::unique_ptr service_manager_; + + /** + * @name Simulation Manager constructor + * @param[in] world_yaml_file The path to the world.yaml file we wish to load + * @param[in] models_path Path to models directory + * @param[in] world_plugins_path Path to world plugins yaml + * @param[in] update_rate Simulator loop rate + * @param[in] step_size Time to step each iteration + * @param[in] show_viz if to show visualization + * @param[in] viz_pub_rate rate to publish visualization + */ + SimulationManager(std::string world_yaml_file, std::string models_path, + std::string world_plugins_path, double update_rate, + double step_size, bool show_viz, double viz_pub_rate); + + /** + * This method contains the loop that runs the simulation + */ + void Main(); + + /** + * Kill the world + */ + void Shutdown(); +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_SIMULATION_MANAGER_H diff --git a/flatland_server/include/flatland_server/timekeeper.h b/flatland_server/include/flatland_server/timekeeper.h index ea2b81f2..65117f45 100644 --- a/flatland_server/include/flatland_server/timekeeper.h +++ b/flatland_server/include/flatland_server/timekeeper.h @@ -1,100 +1,100 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name timekeeper.h - * @brief Used for simulation time keeping - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_TIME_KEEPER_H -#define FLATLAND_SERVER_TIME_KEEPER_H - -#include -#include - -namespace flatland_server { - -class Timekeeper { - public: - ros::Publisher clock_pub_; ///< the topic to publish the clock - ros::NodeHandle nh_; ///< ROS Node handle - ros::Time time_; ///< simulation time - double max_step_size_; ///< maximum step size - const std::string clock_topic_; ///< the name of the clock topic - - /** - * @brief constructor - */ - Timekeeper(); - - /** - * @brief Step time once with the current set of parameters - */ - void StepTime(); - - /** - * @brief Publish the clock to ROS - */ - void UpdateRosClock() const; - - /** - * @brief Set the maximum step size - * @param[in] step_size The step size - */ - void SetMaxStepSize(double step_size); - - /** - * @return The current simulation time - */ - const ros::Time& GetSimTime() const; - - /** - * @return The current step size used for the world - */ - double GetStepSize() const; - - /** - * @return the max step size possible - */ - double GetMaxStepSize() const; -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_TIME_KEEPER_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name timekeeper.h + * @brief Used for simulation time keeping + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_TIME_KEEPER_H +#define FLATLAND_SERVER_TIME_KEEPER_H + +#include +#include + +namespace flatland_server { + +class Timekeeper { + public: + ros::Publisher clock_pub_; ///< the topic to publish the clock + ros::NodeHandle nh_; ///< ROS Node handle + ros::Time time_; ///< simulation time + double max_step_size_; ///< maximum step size + const std::string clock_topic_; ///< the name of the clock topic + + /** + * @brief constructor + */ + Timekeeper(); + + /** + * @brief Step time once with the current set of parameters + */ + void StepTime(); + + /** + * @brief Publish the clock to ROS + */ + void UpdateRosClock() const; + + /** + * @brief Set the maximum step size + * @param[in] step_size The step size + */ + void SetMaxStepSize(double step_size); + + /** + * @return The current simulation time + */ + const ros::Time& GetSimTime() const; + + /** + * @return The current step size used for the world + */ + double GetStepSize() const; + + /** + * @return the max step size possible + */ + double GetMaxStepSize() const; +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_TIME_KEEPER_H diff --git a/flatland_server/include/flatland_server/types.h b/flatland_server/include/flatland_server/types.h index fef728f7..e86a70f1 100644 --- a/flatland_server/include/flatland_server/types.h +++ b/flatland_server/include/flatland_server/types.h @@ -1,137 +1,137 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name types.h - * @brief Defines common types in flatland - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#ifndef FLATLAND_SERVER_TYPES_H -#define FLATLAND_SERVER_TYPES_H - -namespace flatland_server { - -struct Vec2 { - double x; - double y; - - Vec2(double x, double y) { - this->x = x; - this->y = y; - } - - Vec2() : x(0), y(0) {} - - b2Vec2 Box2D() const { return b2Vec2(x, y); } -}; - -struct LineSegment { - Vec2 start; - Vec2 end; - - LineSegment(const Vec2 &start, const Vec2 &end) { - this->start = start; - this->end = end; - } - - LineSegment() { - this->start = Vec2(0, 0); - this->end = Vec2(0, 0); - } -}; - -struct Pose { - double x; - double y; - double theta; ///< theta - - Pose(double x, double y, double theta) { - this->x = x; - this->y = y; - this->theta = theta; - } - - Pose(const std::array &p) { - this->x = p[0]; - this->y = p[1]; - this->theta = p[3]; - } - - Pose() : x(0), y(0), theta(0) {} - - bool operator==(const Pose &p) const { - return x == p.x && y == p.y && theta == p.theta; - } - - bool operator!=(const Pose &p) const { return !operator==(p); } -}; - -struct Color { - double r, g, b, a; - - Color() : r(0), g(0), b(0), a(0) {} - - Color(double r, double g, double b, double a) { - this->r = r; - this->g = g; - this->b = b; - this->a = a; - } - - Color(const std::array &c) { - this->r = c[0]; - this->g = c[1]; - this->b = c[2]; - this->a = c[3]; - } - - bool operator==(const Color &c) const { - return r == c.r && g == c.g && b == c.b && a == c.a; - } - - bool operator!=(const Color &c) const { return !operator==(c); } -}; -} - -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name types.h + * @brief Defines common types in flatland + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#ifndef FLATLAND_SERVER_TYPES_H +#define FLATLAND_SERVER_TYPES_H + +namespace flatland_server { + +struct Vec2 { + double x; + double y; + + Vec2(double x, double y) { + this->x = x; + this->y = y; + } + + Vec2() : x(0), y(0) {} + + b2Vec2 Box2D() const { return {(float)x, (float)y}; } +}; + +struct LineSegment { + Vec2 start; + Vec2 end; + + LineSegment(const Vec2 &start, const Vec2 &end) { + this->start = start; + this->end = end; + } + + LineSegment() { + this->start = Vec2(0, 0); + this->end = Vec2(0, 0); + } +}; + +struct Pose { + double x; + double y; + double theta; ///< theta + + Pose(double x, double y, double theta) { + this->x = x; + this->y = y; + this->theta = theta; + } + + Pose(const std::array &p) { + this->x = p[0]; + this->y = p[1]; + this->theta = p[3]; + } + + Pose() : x(0), y(0), theta(0) {} + + bool operator==(const Pose &p) const { + return x == p.x && y == p.y && theta == p.theta; + } + + bool operator!=(const Pose &p) const { return !operator==(p); } +}; + +struct Color { + double r, g, b, a; + + Color() : r(0), g(0), b(0), a(0) {} + + Color(double r, double g, double b, double a) { + this->r = r; + this->g = g; + this->b = b; + this->a = a; + } + + Color(const std::array &c) { + this->r = c[0]; + this->g = c[1]; + this->b = c[2]; + this->a = c[3]; + } + + bool operator==(const Color &c) const { + return r == c.r && g == c.g && b == c.b && a == c.a; + } + + bool operator!=(const Color &c) const { return !operator==(c); } +}; +} + +#endif diff --git a/flatland_server/include/flatland_server/world.h b/flatland_server/include/flatland_server/world.h index 194bb856..f935c4ac 100644 --- a/flatland_server/include/flatland_server/world.h +++ b/flatland_server/include/flatland_server/world.h @@ -1,210 +1,213 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world.h - * @brief Definition for the simulation world - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_WORLD_H -#define FLATLAND_SERVER_WORLD_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -/** - * This class defines a world in the simulation. A world contains layers - * that can represent environments at multiple levels, and models which are - * can be robots or obstacles. - */ -class World : public b2ContactListener { - public: - boost::filesystem::path world_yaml_dir_; ///, Layer *> - layers_name_map_; ///< map of all layers and thier name - std::vector layers_; ///< list of layers - std::vector models_; ///< list of models - CollisionFilterRegistry cfr_; ///< collision registry for layers and models - PluginManager plugin_manager_; ///< for loading and updating plugins - bool service_paused_; ///< indicates if simulation is paused by a service - /// call or not - InteractiveMarkerManager - int_marker_manager_; ///< for dynamically moving models from Rviz - int physics_position_iterations_; ///< Box2D solver param - int physics_velocity_iterations_; ///< Box2D solver param - - /** - * @brief Constructor for the world class. All data required for - * initialization should be passed in here - */ - World(); - - /** - * @brief Destructor for the world class - */ - ~World(); - - /** - * @brief trigger world update include all physics and plugins - * @param[in] timekeeper The time keeping object - */ - void Update(Timekeeper &timekeeper); - - /** - * @brief Box2D inherited begin contact - * @param[in] contact Box2D contact information - */ - void BeginContact(b2Contact *contact) override; - - /** - * @brief Box2D inherited end contact - * @param[in] contact Box2D contact information - */ - void EndContact(b2Contact *contact) override; - - /** - * @brief Box2D inherited presolve - * @param[in] contact Box2D contact information - * @param[in] oldManifold The manifold from the previous timestep - */ - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold); - - /** - * @brief Box2D inherited pre solve - * @param[in] contact Box2D contact information - * @param[in] impulse The calculated impulse from the collision resolute - */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse); - - /* - * @brief Load world plugins - * @param[in] world_plugin_reader, readin the info about the plugin - * @param[in] world, the world where the plugin will be applied to - * @param[in] world config, the yaml reader of world.yaml - */ - void LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, - YamlReader &world_config); - /** - * @brief load layers into the world. Throws YAMLException. - * @param[in] layers_reader Yaml reader for node that has list of layers - */ - void LoadLayers(YamlReader &layers_reader); - - /** - * @brief load models into the world. Throws YAMLException. - * @param[in] layers_reader Yaml reader for node that has a list of models - */ - void LoadModels(YamlReader &models_reader); - - /** - * @brief load models into the world. Throws YAMLException. - * @param[in] model_yaml_path Relative path to the model yaml file - * @param[in] ns Namespace of the robot - * @param[in] name Name of the model - * @param[in] pose Initial pose of the model in x, y, yaw - */ - void LoadModel(const std::string &model_yaml_path, const std::string &ns, - const std::string &name, const Pose &pose); - - /** - * @brief remove model with a given name - * @param[in] name The name of the model to remove - */ - void DeleteModel(const std::string &name); - - /** - * @brief move model with a given name - * @param[in] name The name of the model to move - * @param[in] pose The desired new pose of the model - */ - void MoveModel(const std::string &name, const Pose &pose); - - /** - * @brief set the paused state of the simulation to true - */ - void Pause(); - - /** - * @brief set the paused state of the simulation to false - */ - void Resume(); - - /** - * @brief toggle the paused state of the simulation - */ - void TogglePaused(); - - /** - * @brief returns true if service_paused_ is true or an interactive marker is - * currently being dragged - */ - bool IsPaused(); - - /** - * @brief factory method to create a instance of the world class. Cleans all - * the inputs before instantiation of the class. TThrows YAMLException. - * @param[in] yaml_path Path to the world yaml file - * @return pointer to a new world - */ - static World *MakeWorld(const std::string &yaml_path); - - /** - * @brief Publish debug visualizations for everything - * @param[in] update_layers since layers are pretty much static, this - * parameter is used to skip updating layers - */ - void DebugVisualize(bool update_layers = true); -}; -}; // namespace flatland_server -#endif // FLATLAND_SERVER_WORLD_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world.h + * @brief Definition for the simulation world + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_WORLD_H +#define FLATLAND_SERVER_WORLD_H + +#include +#include "TaskScheduler.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +/** + * This class defines a world in the simulation. A world contains layers + * that can represent environments at multiple levels, and models which are + * can be robots or obstacles. + */ +class World { + public: + boost::filesystem::path world_yaml_dir_; ///, Layer *> + layers_name_map_; ///< map of all layers and thier name + std::vector layers_; ///< list of layers + std::vector models_; ///< list of models + CollisionFilterRegistry cfr_; ///< collision registry for layers and models + PluginManager plugin_manager_; ///< for loading and updating plugins + bool service_paused_; ///< indicates if simulation is paused by a service + /// call or not + InteractiveMarkerManager + int_marker_manager_; ///< for dynamically moving models from Rviz + int physics_velocity_iterations_; ///< Box2D solver velocity iterations + int physics_position_iterations_; ///< Box2D solver position iterations + enki::TaskScheduler task_scheduler_; ///< enkiTS multi-core task scheduler + MessageServer message_server; ///< internal message passing system + + /** + * @brief Constructor for the world class. All data required for + * initialization should be passed in here + */ + World(); + + /** + * @brief Destructor for the world class + */ + ~World(); + + /** + * @brief trigger world update include all physics and plugins + * @param[in] timekeeper The time keeping object + */ + void Update(Timekeeper &timekeeper); + + /* + * @brief Load world plugins + * @param[in] world_plugin_reader, readin the info about the plugin + * @param[in] world, the world where the plugin will be applied to + * @param[in] world config, the yaml reader of world.yaml + */ + void LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, + YamlReader &world_config); + /** + * @brief load layers into the world. Throws YAMLException. + * @param[in] layers_reader Yaml reader for node that has list of layers + */ + void LoadLayers(YamlReader &layers_reader); + + /** + * @brief load models into the world. Throws YAMLException. + * @param[in] layers_reader Yaml reader for node that has a list of models + */ + void LoadModels(YamlReader &models_reader); + + /** + * @brief load models into the world. Throws YAMLException. + * @param[in] model_yaml_path Relative path to the model yaml file + * @param[in] ns Namespace of the robot + * @param[in] name Name of the model + * @param[in] pose Initial pose of the model in x, y, yaw + */ + void LoadModel(const std::string &model_yaml_path, const std::string &ns, + const std::string &name, const Pose &pose); + + /// @brief Reload layers and models from the world YAML (used by map_info plugin) + void LoadWorldEntities(); + + /// @brief Mark an agent as needing slower simulation time + void SlowSimTime(const std::string &agent); + + /// @brief Remove an agent from the slow-simulation set + void FastSimTime(const std::string &agent); + + /// @brief Initialize dynamic fast-sim parameters (stub — not yet ported to Box2D v3) + void InitializeDynamicFastSim(double max_lower_speed, double min_lower_speed, + int num_robots_threshold); + + /** + * @brief remove model with a given name + * @param[in] name The name of the model to remove + */ + void DeleteModel(const std::string &name); + + /** + * @brief move model with a given name + * @param[in] name The name of the model to move + * @param[in] pose The desired new pose of the model + */ + void MoveModel(const std::string &name, const Pose &pose); + + /** + * @brief set the paused state of the simulation to true + */ + void Pause(); + + /** + * @brief set the paused state of the simulation to false + */ + void Resume(); + + /** + * @brief toggle the paused state of the simulation + */ + void TogglePaused(); + + /** + * @brief returns true if service_paused_ is true or an interactive marker is + * currently being dragged + */ + bool IsPaused(); + + /** + * @brief factory method to create a instance of the world class. Cleans all + * the inputs before instantiation of the class. TThrows YAMLException. + * @param[in] yaml_path Path to the world yaml file + * @return pointer to a new world + */ + static World *MakeWorld(const std::string &yaml_path); + + /** + * @brief Create a world from separate world yaml, models path and plugins yaml. + * @param[in] yaml_path Path to world yaml (layers/models) + * @param[in] models_path Path to models directory + * @param[in] world_plugins_path Path to world plugins yaml + */ + static World *MakeWorld(const std::string &yaml_path, + const std::string &models_path, + const std::string &world_plugins_path); + + /** + * @brief Publish debug visualizations for everything + * @param[in] update_layers since layers are pretty much static, this + * parameter is used to skip updating layers + */ + void DebugVisualize(bool update_layers = true); +}; +}; // namespace flatland_server +#endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/world_plugin.h b/flatland_server/include/flatland_server/world_plugin.h index a2fce502..47f11450 100644 --- a/flatland_server/include/flatland_server/world_plugin.h +++ b/flatland_server/include/flatland_server/world_plugin.h @@ -1,85 +1,87 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_plugin.h - * @brief Interface for WorldPlugin pluginlib plugins - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_WORLD_PLUGIN_H -#define FLATLAND_SERVER_WORLD_PLUGIN_H - -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { -// forward declaration -class World; -class WorldPlugin : public FlatlandPlugin { - protected: - World *world_; - YamlReader world_config_; - - public: - /* - * @brief WorldPlugin default constructor - */ - WorldPlugin() = default; - - /* - * @brief initialize the plugin - * @param[in] world, the World the plugin is attached to - * @param[in] name, name of the plugin - * @param[in] type, type of the plugin - * @param[in] plugin_reader, the YAML node contain the plugin's config - * @param[in] world_config, the yaml reader of world.yaml - */ - void Initialize(World *world, std::string name, std::string type, - YAML::Node &plugin_reader, YamlReader &world_config); -}; -}; - -#endif // FLATLAND_SERVER_WORLD_PLUGIN_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_plugin.h + * @brief Interface for WorldPlugin pluginlib plugins + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_WORLD_PLUGIN_H +#define FLATLAND_SERVER_WORLD_PLUGIN_H + +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { +// forward declaration +class World; +class WorldPlugin : public FlatlandPlugin { + protected: + World *world_; + YamlReader world_config_; + + public: + /// @brief Returns the world this plugin is attached to + World *GetWorld() const { return world_; } + /* + * @brief WorldPlugin default constructor + */ + WorldPlugin() = default; + + /* + * @brief initialize the plugin + * @param[in] world, the World the plugin is attached to + * @param[in] name, name of the plugin + * @param[in] type, type of the plugin + * @param[in] plugin_reader, the YAML node contain the plugin's config + * @param[in] world_config, the yaml reader of world.yaml + */ + void Initialize(World *world, std::string name, std::string type, + YAML::Node &plugin_reader, YamlReader &world_config); +}; +}; + +#endif // FLATLAND_SERVER_WORLD_PLUGIN_H diff --git a/flatland_server/include/flatland_server/yaml_preprocessor.h b/flatland_server/include/flatland_server/yaml_preprocessor.h index 9bcf3e32..537c9c25 100644 --- a/flatland_server/include/flatland_server/yaml_preprocessor.h +++ b/flatland_server/include/flatland_server/yaml_preprocessor.h @@ -1,106 +1,106 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2018 Avidbots Corp. - * @name yaml_preprocessor.h - * @brief Yaml preprocessor using Lua - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_YAML_PREPROCESSOR_H -#define FLATLAND_SERVER_YAML_PREPROCESSOR_H - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -/** - */ -namespace YamlPreprocessor { -/** - * @brief Preprocess with a given node - * @param[in/out] node A Yaml node to parse - * @return The parsed YAML::Node - */ -void Parse(YAML::Node &node); - -/** - * @brief Constructor with a given path to a yaml file, throws exception on - * failure - * @param[in] path Path to the yaml file - * @return The parsed YAML::Node - */ -YAML::Node LoadParse(const std::string &path); - -/** - * @brief Find and run any $eval nodes - * @param[in/out] node A Yaml node to recursively parse - */ -void ProcessNodes(YAML::Node &node); - -/** - * @brief Find and run any $eval expressions - * @param[in/out] node A Yaml string node to parse - */ -void ProcessScalarNode(YAML::Node &node); - -/** - * @brief Get an environment variable with an optional default value - * @param[in/out] lua_State The lua state/stack to read/write to/from - */ -int LuaGetEnv(lua_State *L); - -/** - * @brief Get a rosparam with an optional default value - * @param[in/out] lua_State The lua state/stack to read/write to/from - */ -int LuaGetParam(lua_State *L); -}; -} - -#endif // FLATLAND_SERVER_YAML_PREPROCESSOR_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2018 Avidbots Corp. + * @name yaml_preprocessor.h + * @brief Yaml preprocessor using Lua + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_YAML_PREPROCESSOR_H +#define FLATLAND_SERVER_YAML_PREPROCESSOR_H + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +/** + */ +namespace YamlPreprocessor { +/** + * @brief Preprocess with a given node + * @param[in/out] node A Yaml node to parse + * @return The parsed YAML::Node + */ +void Parse(YAML::Node &node); + +/** + * @brief Constructor with a given path to a yaml file, throws exception on + * failure + * @param[in] path Path to the yaml file + * @return The parsed YAML::Node + */ +YAML::Node LoadParse(const std::string &path); + +/** + * @brief Find and run any $eval nodes + * @param[in/out] node A Yaml node to recursively parse + */ +void ProcessNodes(YAML::Node &node); + +/** + * @brief Find and run any $eval expressions + * @param[in/out] node A Yaml string node to parse + */ +void ProcessScalarNode(YAML::Node &node); + +/** + * @brief Get an environment variable with an optional default value + * @param[in/out] lua_State The lua state/stack to read/write to/from + */ +int LuaGetEnv(lua_State *L); + +/** + * @brief Get a rosparam with an optional default value + * @param[in/out] lua_State The lua state/stack to read/write to/from + */ +int LuaGetParam(lua_State *L); +}; +} + +#endif // FLATLAND_SERVER_YAML_PREPROCESSOR_H diff --git a/flatland_server/include/flatland_server/yaml_reader.h b/flatland_server/include/flatland_server/yaml_reader.h index 892d69b3..af33eb3d 100644 --- a/flatland_server/include/flatland_server/yaml_reader.h +++ b/flatland_server/include/flatland_server/yaml_reader.h @@ -1,472 +1,472 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name yaml_reader.h - * @brief Defines yaml_reader - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_SERVER_YAML_READER_H -#define FLATLAND_SERVER_YAML_READER_H - -#include -#include -#include -#include -#include -#include -#include - -// If we have a version of boost with type_index -#if BOOST_VERSION / 100 % 1000 >= 56 -#include -#define TYPESTRING(T) (boost::typeindex::type_id().pretty_name()) -#else -#include -#include -#define TYPESTRING(T) (typeid(T).name()) -#endif - -#include -#include -#include - -namespace flatland_server { - -/** - */ -class YamlReader { - public: - enum NodeTypeCheck { MAP, LIST, NO_CHECK }; - - YAML::Node node_; ///< The YAML Node this processes - std::set accessed_keys_; /// Records of the keys processed - ///< location of the entry, used to show where the error come from - std::string filename_; - std::string file_path_; - std::string entry_location_; - ///< name of the yaml entry, used to show where the error come from - std::string entry_name_; - std::string fmt_in_; ///< formatted entry location for display - std::string fmt_name_; ///< formatted entry location for display - - /** - * @brief Default constructor for yaml reader, initialize with a empty yaml - * Node - */ - YamlReader(); - - /** - * @brief Constructor with a given node - * @param[in] node A Yaml node to get data from - */ - YamlReader(const YAML::Node &node); - - /** - * @brief Constructor with a given path to a yaml file, throws exception on - * failure - * @param[in] path Path to the yaml file - */ - YamlReader(const std::string &path); - - /** - * @brief Use this method to set the entry location and entry name for error - * message purposes - * @param[in] entry_location Location of the entry, use "_NONE_" for no entry, - * empty string indicates keep current value - * @param[in] entry_name Name of the entry, optional, use "_NONE_" for no - * entry, empty string indicates keep current value - */ - void SetErrorInfo(std::string entry_location, std::string entry_name = ""); - - /** - * @brief Use this method to set the file location of the yaml node - * @param[in] file_path path to the file, use "_NONE_" for empty string, - * file_path with empty string will keep current path - */ - void SetFile(const std::string &file_path); - - /** - * @brief This method checks all keys in the yaml node are used, otherwise it - * throws an exception - */ - void EnsureAccessedAllKeys(); - - /** - * @return The yaml cpp Yaml Node - */ - YAML::Node Node(); - - /** - * @return If the node is null, a.k.a if it is empty - */ - bool IsNodeNull(); - - /** - * @return The number of subnodes in the node, a.k.a size of the node - */ - int NodeSize(); - - /** - * @brief Get one of the subnode using a index, throws exception on failure, - * file path is inherited from the parent - * @param[in] index Index of the subnode - * @param[in] type_check Check if the type of the subnode, i.e. list or map - * @param[in] sub_node_location The error location of the subnode, optional, - * if not provided or a given an empty string, it will generate the location - * using its parents entry location and entry name. It also accepts "_NONE_" - * @return YamlReader of the sub node - */ - YamlReader Subnode(int index, NodeTypeCheck type_check, - std::string sub_node_location = ""); - /** - * @brief Get one of the subnode using a key, throws exception on failure, - * file path is inherited from the parent - * @param[in] key Key of the subnode - * @param[in] type_check Check if the type of the subnode, i.e. list or map - * @param[in] sub_node_location The error location of the subnode, optional, - * if not provided or a given an empty string, it will generate the location - * using its parents entry location and entry name. It also accepts "_NONE_" - * @return YamlReader of the sub node - */ - YamlReader Subnode(const std::string &key, NodeTypeCheck type_check, - std::string sub_node_location = ""); - /** - * @brief Optionally get one of the subnode using a key, throws exception on - * failure, file path is inherited from the parent - * @param[in] key Key of the subnode - * @param[in] type_check Check if the type of the subnode, i.e. list or map - * @param[in] sub_node_location The error location of the subnode, optional, - * if not provided or a given an empty string, it will generate the location - * using its parents entry location and entry name. It also accepts "_NONE_" - * @return YamlReader of the sub node, or a YamlReader of a empty node if a - * node with the given key does not exist - */ - YamlReader SubnodeOpt(const std::string &key, NodeTypeCheck type_check, - std::string sub_node_location = ""); - /** - * @brief Convert the current node in yaml reader to a given template type. It - * uses yaml-cpp's as() function, you could specify conversion for custom - * datatypes using yaml cpp's documented ways, this method throws exception on - * failure - * @return The value after conversion. - */ - template - T As(); - - /** - * @brief Convert the current node to a list of given type, throws exception - * on failure - * @return List of given type - */ - template - std::vector AsList(int min_size, int max_size); - - /** - * @brief Convert the current node to a array of given type, throws exception - * on failure - * @return Array of given type - */ - template - std::array AsArray(); - - /** - * @brief Get subnode with a given key and converted to the given type, throws - * on failure - * @param[in] key Key to access the subnode - * @return Value of the converted subnode - */ - template - T Get(const std::string &key); - - /** - * @brief Optionally get subnode with a given key and converted to the given - * type, throws on failure - * @param[in] key Key to access the subnode - * @param[in] default_val Default value - * @return Value of the converted subnode, or default value if node with key - * does not exist - */ - template - T Get(const std::string &key, const T &default_val); - - /** - * @brief Get subnode with a given key and converted to list of the given - * type, throws on failure - * @param[in] key Key to access the subnode - * @param[in] min_size Minimum size of the list, -1 to ignore - * @param[in] max_size Maximum size of the list, -1 to ignore - * @return Value of the converted subnode - */ - template - std::vector GetList(const std::string &key, int min_size, int max_size); - - /** - * @brief Optionally get subnode with a given key and converted to list of the - * given type, throws on failure - * @param[in] key Key to access the subnode - * @param[in] default_val Default value - * @param[in] min_size Minimum size of the list, -1 to ignore - * @param[in] max_size Maximum size of the list, -1 to ignore - * @return Value of the converted subnode - */ - template - std::vector GetList(const std::string &key, - const std::vector default_val, int min_size, - int max_size); - - /** - * @brief Get subnode with a given key and converted to array of the given - * type, throws on failure - * @param[in] key Key to access the subnode - * @return Value of the converted subnode - */ - template - std::array GetArray(const std::string &key); - - /** - * @brief Optionally get subnode with a given key and converted to array of - * a given type, throws on failure - * @param[in] key Key to access the subnode - * @param[in] default_val Default value - * @return Value of the converted subnode - */ - template - std::array GetArray(const std::string &key, - const std::array default_val); - - /** - * @return A Vec2 accessed by a given key - */ - Vec2 GetVec2(const std::string &key); - - /** - * @return A Vec2 accessed by a given key, or default value if key does not - * exist - */ - Vec2 GetVec2(const std::string &key, const Vec2 &default_val); - - /** - * @return A Color value accessed by a given key, or default value if key does - * not exist - */ - Color GetColor(const std::string &key, const Color &default_val); - - /** - * @return A Pose value accessed by a given key - */ - Pose GetPose(const std::string &key); - - /** - * @return A Pose value accessed by a given key, or default value if key does - * not exist - */ - Pose GetPose(const std::string &key, const Pose &default_val); -}; - -/** - * @return A string with quotes around the input - */ -inline std::string Q(const std::string &str) { return "\"" + str + "\""; } - -template -T YamlReader::As() { - T ret; - - try { - ret = node_.as(); - } catch (const YAML::RepresentationException &e) { - throw YAMLException("Error converting entry" + fmt_name_ + " to " + - TYPESTRING(T) + fmt_in_); - } catch (const YAML::Exception &e) { - throw YAMLException("Error reading entry" + fmt_name_ + fmt_in_); - } - - return ret; -} - -template -std::vector YamlReader::AsList(int min_size, int max_size) { - std::vector list; - - if (min_size > 0 && max_size > 0 && min_size == max_size && - NodeSize() != max_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size of exactly " + - std::to_string(min_size) + fmt_in_); - } - - if (min_size > 0 && NodeSize() < min_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size >= " + - std::to_string(min_size) + fmt_in_); - } - - if (max_size > 0 && NodeSize() > max_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size <= " + - std::to_string(max_size) + fmt_in_); - } - - for (int i = 0; i < NodeSize(); i++) { - list.push_back(Subnode(i, NO_CHECK).As()); - } - - return list; -} - -template -std::array YamlReader::AsArray() { - std::vector list_ret = AsList(N, N); - std::array array_ret; - - for (int i = 0; i < N; i++) { - array_ret[i] = list_ret[i]; - } - return array_ret; -} - -template -T YamlReader::Get(const std::string &key) { - return Subnode(key, NO_CHECK).As(); -} - -template -T YamlReader::Get(const std::string &key, const T &default_val) { - if (!node_[key]) { - accessed_keys_.insert(key); - return default_val; - } - return Get(key); -} - -template -std::vector YamlReader::GetList(const std::string &key, int min_size, - int max_size) { - return Subnode(key, LIST).AsList(min_size, max_size); -} - -template -std::vector YamlReader::GetList(const std::string &key, - const std::vector default_val, - int min_size, int max_size) { - if (!node_[key]) { - accessed_keys_.insert(key); - return default_val; - } - - return GetList(key, min_size, max_size); -} - -template -std::array YamlReader::GetArray(const std::string &key) { - return Subnode(key, LIST).AsArray(); -} - -template -std::array YamlReader::GetArray(const std::string &key, - const std::array default_val) { - if (!node_[key]) { - accessed_keys_.insert(key); - return default_val; - } - - return GetArray(key); -} -} - -// encode and decode functions for yaml-cpp to convert values for commonly used -// types in flatland server -namespace YAML { -template <> -struct convert { - static bool decode(const Node &node, b2Vec2 &rhs) { - if (!node.IsSequence() || node.size() != 2) { - return false; - } - - rhs.x = node[0].as(); - rhs.y = node[1].as(); - return true; - } -}; - -template <> -struct convert { - static bool decode(const Node &node, flatland_server::Vec2 &rhs) { - if (!node.IsSequence() || node.size() != 2) { - return false; - } - - rhs.x = node[0].as(); - rhs.y = node[1].as(); - return true; - } -}; - -template <> -struct convert { - static bool decode(const Node &node, flatland_server::Color &rhs) { - if (!node.IsSequence() || node.size() != 4) { - return false; - } - - rhs.r = node[0].as(); - rhs.g = node[1].as(); - rhs.b = node[2].as(); - rhs.a = node[3].as(); - return true; - } -}; - -template <> -struct convert { - static bool decode(const Node &node, flatland_server::Pose &rhs) { - if (!node.IsSequence() || node.size() != 3) { - return false; - } - - rhs.x = node[0].as(); - rhs.y = node[1].as(); - rhs.theta = node[2].as(); - return true; - } -}; -} - -#endif +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name yaml_reader.h + * @brief Defines yaml_reader + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_SERVER_YAML_READER_H +#define FLATLAND_SERVER_YAML_READER_H + +#include +#include +#include +#include +#include +#include +#include + +// If we have a version of boost with type_index +#if BOOST_VERSION / 100 % 1000 >= 56 +#include +#define TYPESTRING(T) (boost::typeindex::type_id().pretty_name()) +#else +#include +#include +#define TYPESTRING(T) (typeid(T).name()) +#endif + +#include +#include +#include + +namespace flatland_server { + +/** + */ +class YamlReader { + public: + enum NodeTypeCheck { MAP, LIST, NO_CHECK }; + + YAML::Node node_; ///< The YAML Node this processes + std::set accessed_keys_; /// Records of the keys processed + ///< location of the entry, used to show where the error come from + std::string filename_; + std::string file_path_; + std::string entry_location_; + ///< name of the yaml entry, used to show where the error come from + std::string entry_name_; + std::string fmt_in_; ///< formatted entry location for display + std::string fmt_name_; ///< formatted entry location for display + + /** + * @brief Default constructor for yaml reader, initialize with a empty yaml + * Node + */ + YamlReader(); + + /** + * @brief Constructor with a given node + * @param[in] node A Yaml node to get data from + */ + YamlReader(const YAML::Node &node); + + /** + * @brief Constructor with a given path to a yaml file, throws exception on + * failure + * @param[in] path Path to the yaml file + */ + YamlReader(const std::string &path); + + /** + * @brief Use this method to set the entry location and entry name for error + * message purposes + * @param[in] entry_location Location of the entry, use "_NONE_" for no entry, + * empty string indicates keep current value + * @param[in] entry_name Name of the entry, optional, use "_NONE_" for no + * entry, empty string indicates keep current value + */ + void SetErrorInfo(std::string entry_location, std::string entry_name = ""); + + /** + * @brief Use this method to set the file location of the yaml node + * @param[in] file_path path to the file, use "_NONE_" for empty string, + * file_path with empty string will keep current path + */ + void SetFile(const std::string &file_path); + + /** + * @brief This method checks all keys in the yaml node are used, otherwise it + * throws an exception + */ + void EnsureAccessedAllKeys(); + + /** + * @return The yaml cpp Yaml Node + */ + YAML::Node Node(); + + /** + * @return If the node is null, a.k.a if it is empty + */ + bool IsNodeNull(); + + /** + * @return The number of subnodes in the node, a.k.a size of the node + */ + int NodeSize(); + + /** + * @brief Get one of the subnode using a index, throws exception on failure, + * file path is inherited from the parent + * @param[in] index Index of the subnode + * @param[in] type_check Check if the type of the subnode, i.e. list or map + * @param[in] sub_node_location The error location of the subnode, optional, + * if not provided or a given an empty string, it will generate the location + * using its parents entry location and entry name. It also accepts "_NONE_" + * @return YamlReader of the sub node + */ + YamlReader Subnode(int index, NodeTypeCheck type_check, + std::string sub_node_location = ""); + /** + * @brief Get one of the subnode using a key, throws exception on failure, + * file path is inherited from the parent + * @param[in] key Key of the subnode + * @param[in] type_check Check if the type of the subnode, i.e. list or map + * @param[in] sub_node_location The error location of the subnode, optional, + * if not provided or a given an empty string, it will generate the location + * using its parents entry location and entry name. It also accepts "_NONE_" + * @return YamlReader of the sub node + */ + YamlReader Subnode(const std::string &key, NodeTypeCheck type_check, + std::string sub_node_location = ""); + /** + * @brief Optionally get one of the subnode using a key, throws exception on + * failure, file path is inherited from the parent + * @param[in] key Key of the subnode + * @param[in] type_check Check if the type of the subnode, i.e. list or map + * @param[in] sub_node_location The error location of the subnode, optional, + * if not provided or a given an empty string, it will generate the location + * using its parents entry location and entry name. It also accepts "_NONE_" + * @return YamlReader of the sub node, or a YamlReader of a empty node if a + * node with the given key does not exist + */ + YamlReader SubnodeOpt(const std::string &key, NodeTypeCheck type_check, + std::string sub_node_location = ""); + /** + * @brief Convert the current node in yaml reader to a given template type. It + * uses yaml-cpp's as() function, you could specify conversion for custom + * datatypes using yaml cpp's documented ways, this method throws exception on + * failure + * @return The value after conversion. + */ + template + T As(); + + /** + * @brief Convert the current node to a list of given type, throws exception + * on failure + * @return List of given type + */ + template + std::vector AsList(int min_size, int max_size); + + /** + * @brief Convert the current node to a array of given type, throws exception + * on failure + * @return Array of given type + */ + template + std::array AsArray(); + + /** + * @brief Get subnode with a given key and converted to the given type, throws + * on failure + * @param[in] key Key to access the subnode + * @return Value of the converted subnode + */ + template + T Get(const std::string &key); + + /** + * @brief Optionally get subnode with a given key and converted to the given + * type, throws on failure + * @param[in] key Key to access the subnode + * @param[in] default_val Default value + * @return Value of the converted subnode, or default value if node with key + * does not exist + */ + template + T Get(const std::string &key, const T &default_val); + + /** + * @brief Get subnode with a given key and converted to list of the given + * type, throws on failure + * @param[in] key Key to access the subnode + * @param[in] min_size Minimum size of the list, -1 to ignore + * @param[in] max_size Maximum size of the list, -1 to ignore + * @return Value of the converted subnode + */ + template + std::vector GetList(const std::string &key, int min_size, int max_size); + + /** + * @brief Optionally get subnode with a given key and converted to list of the + * given type, throws on failure + * @param[in] key Key to access the subnode + * @param[in] default_val Default value + * @param[in] min_size Minimum size of the list, -1 to ignore + * @param[in] max_size Maximum size of the list, -1 to ignore + * @return Value of the converted subnode + */ + template + std::vector GetList(const std::string &key, + const std::vector default_val, int min_size, + int max_size); + + /** + * @brief Get subnode with a given key and converted to array of the given + * type, throws on failure + * @param[in] key Key to access the subnode + * @return Value of the converted subnode + */ + template + std::array GetArray(const std::string &key); + + /** + * @brief Optionally get subnode with a given key and converted to array of + * a given type, throws on failure + * @param[in] key Key to access the subnode + * @param[in] default_val Default value + * @return Value of the converted subnode + */ + template + std::array GetArray(const std::string &key, + const std::array default_val); + + /** + * @return A Vec2 accessed by a given key + */ + Vec2 GetVec2(const std::string &key); + + /** + * @return A Vec2 accessed by a given key, or default value if key does not + * exist + */ + Vec2 GetVec2(const std::string &key, const Vec2 &default_val); + + /** + * @return A Color value accessed by a given key, or default value if key does + * not exist + */ + Color GetColor(const std::string &key, const Color &default_val); + + /** + * @return A Pose value accessed by a given key + */ + Pose GetPose(const std::string &key); + + /** + * @return A Pose value accessed by a given key, or default value if key does + * not exist + */ + Pose GetPose(const std::string &key, const Pose &default_val); +}; + +/** + * @return A string with quotes around the input + */ +inline std::string Q(const std::string &str) { return "\"" + str + "\""; } + +template +T YamlReader::As() { + T ret; + + try { + ret = node_.as(); + } catch (const YAML::RepresentationException &e) { + throw YAMLException("Error converting entry" + fmt_name_ + " to " + + TYPESTRING(T) + fmt_in_); + } catch (const YAML::Exception &e) { + throw YAMLException("Error reading entry" + fmt_name_ + fmt_in_); + } + + return ret; +} + +template +std::vector YamlReader::AsList(int min_size, int max_size) { + std::vector list; + + if (min_size > 0 && max_size > 0 && min_size == max_size && + NodeSize() != max_size) { + throw YAMLException("Entry" + fmt_name_ + " must have size of exactly " + + std::to_string(min_size) + fmt_in_); + } + + if (min_size > 0 && NodeSize() < min_size) { + throw YAMLException("Entry" + fmt_name_ + " must have size >= " + + std::to_string(min_size) + fmt_in_); + } + + if (max_size > 0 && NodeSize() > max_size) { + throw YAMLException("Entry" + fmt_name_ + " must have size <= " + + std::to_string(max_size) + fmt_in_); + } + + for (int i = 0; i < NodeSize(); i++) { + list.push_back(Subnode(i, NO_CHECK).As()); + } + + return list; +} + +template +std::array YamlReader::AsArray() { + std::vector list_ret = AsList(N, N); + std::array array_ret; + + for (int i = 0; i < N; i++) { + array_ret[i] = list_ret[i]; + } + return array_ret; +} + +template +T YamlReader::Get(const std::string &key) { + return Subnode(key, NO_CHECK).As(); +} + +template +T YamlReader::Get(const std::string &key, const T &default_val) { + if (!node_[key]) { + accessed_keys_.insert(key); + return default_val; + } + return Get(key); +} + +template +std::vector YamlReader::GetList(const std::string &key, int min_size, + int max_size) { + return Subnode(key, LIST).AsList(min_size, max_size); +} + +template +std::vector YamlReader::GetList(const std::string &key, + const std::vector default_val, + int min_size, int max_size) { + if (!node_[key]) { + accessed_keys_.insert(key); + return default_val; + } + + return GetList(key, min_size, max_size); +} + +template +std::array YamlReader::GetArray(const std::string &key) { + return Subnode(key, LIST).AsArray(); +} + +template +std::array YamlReader::GetArray(const std::string &key, + const std::array default_val) { + if (!node_[key]) { + accessed_keys_.insert(key); + return default_val; + } + + return GetArray(key); +} +} + +// encode and decode functions for yaml-cpp to convert values for commonly used +// types in flatland server +namespace YAML { +template <> +struct convert { + static bool decode(const Node &node, b2Vec2 &rhs) { + if (!node.IsSequence() || node.size() != 2) { + return false; + } + + rhs.x = node[0].as(); + rhs.y = node[1].as(); + return true; + } +}; + +template <> +struct convert { + static bool decode(const Node &node, flatland_server::Vec2 &rhs) { + if (!node.IsSequence() || node.size() != 2) { + return false; + } + + rhs.x = node[0].as(); + rhs.y = node[1].as(); + return true; + } +}; + +template <> +struct convert { + static bool decode(const Node &node, flatland_server::Color &rhs) { + if (!node.IsSequence() || node.size() != 4) { + return false; + } + + rhs.r = node[0].as(); + rhs.g = node[1].as(); + rhs.b = node[2].as(); + rhs.a = node[3].as(); + return true; + } +}; + +template <> +struct convert { + static bool decode(const Node &node, flatland_server::Pose &rhs) { + if (!node.IsSequence() || node.size() != 3) { + return false; + } + + rhs.x = node[0].as(); + rhs.y = node[1].as(); + rhs.theta = node[2].as(); + return true; + } +}; +} + +#endif diff --git a/flatland_server/launch/benchmark.launch b/flatland_server/launch/benchmark.launch new file mode 100644 index 00000000..2c5f1b5e --- /dev/null +++ b/flatland_server/launch/benchmark.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch index a465b1c7..9d338170 100644 --- a/flatland_server/launch/server.launch +++ b/flatland_server/launch/server.launch @@ -9,9 +9,7 @@ - - - + @@ -23,7 +21,6 @@ - diff --git a/flatland_server/package.xml b/flatland_server/package.xml index 2754b5ee..f1609882 100644 --- a/flatland_server/package.xml +++ b/flatland_server/package.xml @@ -1,7 +1,7 @@ flatland_server - 1.1.3 + 1.5.0 The flatland_server package BSD https://bitbucket.org/avidbots/flatland @@ -19,6 +19,7 @@ pluginlib roscpp std_msgs + std_srvs tf2 tf2_geometry_msgs geometry_msgs diff --git a/scripts/map_to_lines.py b/flatland_server/scripts/map_to_lines.py similarity index 100% rename from scripts/map_to_lines.py rename to flatland_server/scripts/map_to_lines.py diff --git a/flatland_server/src/body.cpp b/flatland_server/src/body.cpp index e1cad5a7..be27ebad 100644 --- a/flatland_server/src/body.cpp +++ b/flatland_server/src/body.cpp @@ -1,106 +1,105 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name body.cpp - * @brief implements flatland body - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -namespace flatland_server { - -Body::Body(b2World *physics_world, Entity *entity, const std::string &name, - const Color &color, const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping, - double angular_damping) - : entity_(entity), name_(name), color_(color), properties_(properties) { - b2BodyDef body_def; - body_def.type = body_type; - body_def.position.Set(pose.x, pose.y); - body_def.angle = pose.theta; - body_def.linearDamping = linear_damping; - body_def.angularDamping = angular_damping; - - physics_body_ = physics_world->CreateBody(&body_def); - physics_body_->SetUserData(this); -} - -Body::~Body() { - if (physics_body_) { - physics_body_->GetWorld()->DestroyBody(physics_body_); - } -} - -int Body::GetFixturesCount() const { - int count = 0; - for (b2Fixture *f = physics_body_->GetFixtureList(); f; f = f->GetNext()) { - count++; - } - - return count; -} - -Entity *Body::GetEntity() { return entity_; } - -const std::string &Body::GetName() const { return name_; } - -b2Body *Body::GetPhysicsBody() { return physics_body_; } - -const Color &Body::GetColor() const { return color_; } - -void Body::SetColor(const Color &color) { color_ = color; } - -void Body::DebugOutput() const { - ROS_DEBUG_NAMED( - "Body", - "Body %p: entity(%p, %s) name(%s) color(%f,%f,%f,%f) " - "physics_body(%p) num_fixtures(%d) type(%d) pose(%f, %f, %f) " - "angular_damping(%f) linear_damping(%f)", - this, entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, - color_.b, color_.a, physics_body_, GetFixturesCount(), - physics_body_->GetType(), physics_body_->GetPosition().x, - physics_body_->GetPosition().y, physics_body_->GetAngle(), - physics_body_->GetAngularDamping(), physics_body_->GetLinearDamping()); -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name body.cpp + * @brief implements flatland body + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +namespace flatland_server { + +Body::Body(b2WorldId physics_world, Entity *entity, const std::string &name, + const Color &color, const Pose &pose, b2BodyType body_type, + const YAML::Node &properties, double linear_damping, + double angular_damping) + : entity_(entity), name_(name), color_(color), properties_(properties), + parent_transform_(pose) { + b2BodyDef body_def = b2DefaultBodyDef(); + body_def.type = body_type; + body_def.position = {static_cast(pose.x), static_cast(pose.y)}; + body_def.rotation = b2MakeRot(static_cast(pose.theta)); + body_def.linearDamping = static_cast(linear_damping); + body_def.angularDamping = static_cast(angular_damping); + + physics_body_ = b2CreateBody(physics_world, &body_def); + b2Body_SetUserData(physics_body_, this); +} + +Body::~Body() { + if (b2Body_IsValid(physics_body_)) { + b2DestroyBody(physics_body_); + } +} + +int Body::GetShapesCount() const { + return b2Body_GetShapeCount(physics_body_); +} + +Entity *Body::GetEntity() { return entity_; } + +const std::string &Body::GetName() const { return name_; } + +b2BodyId Body::GetPhysicsBody() { return physics_body_; } + +const Color &Body::GetColor() const { return color_; } + +void Body::SetColor(const Color &color) { color_ = color; } + +void Body::DebugOutput() const { + b2Vec2 pos = b2Body_GetPosition(physics_body_); + float angle = b2Rot_GetAngle(b2Body_GetRotation(physics_body_)); + ROS_DEBUG_NAMED( + "Body", + "Body %p: entity(%p, %s) name(%s) color(%f,%f,%f,%f) " + "num_shapes(%d) type(%d) pose(%f, %f, %f) " + "angular_damping(%f) linear_damping(%f)", + this, entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, + color_.b, color_.a, GetShapesCount(), + static_cast(b2Body_GetType(physics_body_)), + pos.x, pos.y, angle, + b2Body_GetAngularDamping(physics_body_), + b2Body_GetLinearDamping(physics_body_)); +} + +}; // namespace flatland_server diff --git a/flatland_server/src/collision_filter_registry.cpp b/flatland_server/src/collision_filter_registry.cpp index 6454b05d..43223be4 100644 --- a/flatland_server/src/collision_filter_registry.cpp +++ b/flatland_server/src/collision_filter_registry.cpp @@ -1,149 +1,149 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name collision_filter_registry.cpp - * @brief Implements Collision Filter Registrar - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -namespace flatland_server { - -const int CollisionFilterRegistry::LAYER_NOT_EXIST; -const int CollisionFilterRegistry::LAYER_ALREADY_EXIST; -const int CollisionFilterRegistry::LAYERS_FULL; -const int CollisionFilterRegistry::MAX_LAYERS; - -CollisionFilterRegistry::CollisionFilterRegistry() - : no_collide_group_cnt_(0), collide_group_cnt_(0) {} - -int CollisionFilterRegistry::RegisterCollide() { - collide_group_cnt_++; - return collide_group_cnt_; -} - -int CollisionFilterRegistry::RegisterNoCollide() { - no_collide_group_cnt_--; - return no_collide_group_cnt_; -} - -bool CollisionFilterRegistry::IsLayersFull() const { - return layer_id_table_.size() >= MAX_LAYERS; -} - -int CollisionFilterRegistry::RegisterLayer(std::string layer_name) { - if (IsLayersFull()) { - return LAYERS_FULL; - } - - if (layer_id_table_.count(layer_name) > 0) { - return LAYER_ALREADY_EXIST; - } - - // You have maximum number of ID you can assign. Loop through all the ones - // assigned currently and find on that is not used - int i; - for (i = 0; i < MAX_LAYERS; i++) { - std::map::iterator it; - for (it = layer_id_table_.begin(); it != layer_id_table_.end(); it++) { - if (it->second == i) { - break; - } - } - - if (it == layer_id_table_.end()) { - layer_id_table_[layer_name] = i; - break; - } - } - - return i; -} - -int CollisionFilterRegistry::LookUpLayerId(std::string layer_name) const { - if (layer_id_table_.count(layer_name) == 0) { - return LAYER_NOT_EXIST; - } - return layer_id_table_.at(layer_name); -} - -std::vector CollisionFilterRegistry::GetAllLayers() const { - std::vector layer_names; - - std::map::const_iterator it; - for (it = layer_id_table_.begin(); it != layer_id_table_.end(); it++) { - layer_names.push_back(it->first); - } - - return layer_names; -} - -int CollisionFilterRegistry::LayersCount() const { - return layer_id_table_.size(); -} - -uint16_t CollisionFilterRegistry::GetCategoryBits( - const std::vector &layers, - std::vector *invalid_layers) const { - if (layers.size() == 1 && layers[0] == "all") { - return ~((uint16_t)0x0); - } - - if (invalid_layers) { - invalid_layers->clear(); - } - uint16_t category_bits = 0; - - for (const auto &layer : layers) { - int layer_id = LookUpLayerId(layer); - - if (layer_id < 0 && invalid_layers) { - invalid_layers->push_back(layer); - } else { - category_bits |= 1 << layer_id; - } - } - - return category_bits; -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name collision_filter_registry.cpp + * @brief Implements Collision Filter Registrar + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +namespace flatland_server { + +const int CollisionFilterRegistry::LAYER_NOT_EXIST; +const int CollisionFilterRegistry::LAYER_ALREADY_EXIST; +const int CollisionFilterRegistry::LAYERS_FULL; +const int CollisionFilterRegistry::MAX_LAYERS; + +CollisionFilterRegistry::CollisionFilterRegistry() + : no_collide_group_cnt_(0), collide_group_cnt_(0) {} + +int CollisionFilterRegistry::RegisterCollide() { + collide_group_cnt_++; + return collide_group_cnt_; +} + +int CollisionFilterRegistry::RegisterNoCollide() { + no_collide_group_cnt_--; + return no_collide_group_cnt_; +} + +bool CollisionFilterRegistry::IsLayersFull() const { + return layer_id_table_.size() >= MAX_LAYERS; +} + +int CollisionFilterRegistry::RegisterLayer(std::string layer_name) { + if (IsLayersFull()) { + return LAYERS_FULL; + } + + if (layer_id_table_.count(layer_name) > 0) { + return LAYER_ALREADY_EXIST; + } + + // You have maximum number of ID you can assign. Loop through all the ones + // assigned currently and find on that is not used + int i; + for (i = 0; i < MAX_LAYERS; i++) { + std::map::iterator it; + for (it = layer_id_table_.begin(); it != layer_id_table_.end(); it++) { + if (it->second == i) { + break; + } + } + + if (it == layer_id_table_.end()) { + layer_id_table_[layer_name] = i; + break; + } + } + + return i; +} + +int CollisionFilterRegistry::LookUpLayerId(std::string layer_name) const { + if (layer_id_table_.count(layer_name) == 0) { + return LAYER_NOT_EXIST; + } + return layer_id_table_.at(layer_name); +} + +std::vector CollisionFilterRegistry::GetAllLayers() const { + std::vector layer_names; + + std::map::const_iterator it; + for (it = layer_id_table_.begin(); it != layer_id_table_.end(); it++) { + layer_names.push_back(it->first); + } + + return layer_names; +} + +int CollisionFilterRegistry::LayersCount() const { + return layer_id_table_.size(); +} + +uint16_t CollisionFilterRegistry::GetCategoryBits( + const std::vector &layers, + std::vector *invalid_layers) const { + if (layers.size() == 1 && layers[0] == "all") { + return ~((uint16_t)0x0); + } + + if (invalid_layers) { + invalid_layers->clear(); + } + uint16_t category_bits = 0; + + for (const auto &layer : layers) { + int layer_id = LookUpLayerId(layer); + + if (layer_id < 0 && invalid_layers) { + invalid_layers->push_back(layer); + } else { + category_bits |= 1 << layer_id; + } + } + + return category_bits; +} + +}; // namespace flatland_server diff --git a/flatland_server/src/debug_visualization.cpp b/flatland_server/src/debug_visualization.cpp index f29c8e72..b0e89139 100644 --- a/flatland_server/src/debug_visualization.cpp +++ b/flatland_server/src/debug_visualization.cpp @@ -1,363 +1,346 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name debug_visualization.cpp - * @brief Transform box2d types into published visualization messages - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/debug_visualization.h" -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -DebugVisualization::DebugVisualization() : node_("~debug") { - topic_list_publisher_ = - node_.advertise("topics", 0, true); -} - -DebugVisualization& DebugVisualization::Get() { - static DebugVisualization instance; - return instance; -} - -void DebugVisualization::JointToMarkers( - visualization_msgs::MarkerArray& markers, b2Joint* joint, float r, float g, - float b, float a) { - if (joint->GetType() == e_distanceJoint || - joint->GetType() == e_pulleyJoint || joint->GetType() == e_mouseJoint) { - ROS_ERROR_NAMED("DebugVis", - "Unimplemented visualization joints. See b2World.cpp for " - "implementation"); - return; - } - - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.color.r = r; - marker.color.g = g; - marker.color.b = b; - marker.color.a = a; - marker.type = marker.LINE_LIST; - marker.scale.x = 0.01; - - geometry_msgs::Point p_a1, p_a2, p_b1, p_b2; - p_a1.x = joint->GetAnchorA().x; - p_a1.y = joint->GetAnchorA().y; - p_a2.x = joint->GetAnchorB().x; - p_a2.y = joint->GetAnchorB().y; - p_b1.x = joint->GetBodyA()->GetPosition().x; - p_b1.y = joint->GetBodyA()->GetPosition().y; - p_b2.x = joint->GetBodyB()->GetPosition().x; - p_b2.y = joint->GetBodyB()->GetPosition().y; - - // Visualization shows lines from bodyA to anchorA, bodyB to anchorB, and - // anchorA to anchorB - marker.id = markers.markers.size(); - marker.points.push_back(p_b1); - marker.points.push_back(p_a1); - marker.points.push_back(p_b2); - marker.points.push_back(p_a2); - marker.points.push_back(p_a1); - marker.points.push_back(p_a2); - - markers.markers.push_back(marker); - - marker.id = markers.markers.size(); - marker.type = marker.CUBE_LIST; - marker.scale.x = marker.scale.y = marker.scale.z = 0.03; - marker.points.clear(); - marker.points.push_back(p_a1); - marker.points.push_back(p_a2); - marker.points.push_back(p_b1); - marker.points.push_back(p_b2); - markers.markers.push_back(marker); -} - -void DebugVisualization::BodyToMarkers(visualization_msgs::MarkerArray& markers, - b2Body* body, float r, float g, float b, - float a) { - b2Fixture* fixture = body->GetFixtureList(); - - while (fixture != NULL) { // traverse fixture linked list - visualization_msgs::Marker marker; - marker.header.frame_id = "map"; - marker.id = markers.markers.size(); - marker.color.r = r; - marker.color.g = g; - marker.color.b = b; - marker.color.a = a; - marker.pose.position.x = body->GetPosition().x; - marker.pose.position.y = body->GetPosition().y; - tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion - q.setRPY(0, 0, body->GetAngle()); // from euler angles: roll, pitch, yaw - marker.pose.orientation = tf2::toMsg(q); - bool add_marker = true; - - // Get the shape from the fixture - switch (fixture->GetType()) { - case b2Shape::e_circle: { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); - - marker.type = marker.SPHERE_LIST; - float diameter = circle->m_radius * 2.0; - marker.scale.z = 0.01; - marker.scale.x = diameter; - marker.scale.y = diameter; - - geometry_msgs::Point p; - p.x = circle->m_p.x; - p.y = circle->m_p.y; - marker.points.push_back(p); - - } break; - - case b2Shape::e_polygon: { // Convert b2Polygon -> LINE_STRIP - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - marker.type = marker.LINE_STRIP; - marker.scale.x = 0.03; // 3cm wide lines - - for (int i = 0; i < poly->m_count; i++) { - geometry_msgs::Point p; - p.x = poly->m_vertices[i].x; - p.y = poly->m_vertices[i].y; - marker.points.push_back(p); - } - marker.points.push_back(marker.points[0]); // Close the shape - - } break; - - case b2Shape::e_edge: { // Convert b2Edge -> LINE_LIST - geometry_msgs::Point p; // b2Edge uses vertex1 and 2 for its edges - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); - - // If the last marker is a line list, extend it - if (markers.markers.size() > 0 && - markers.markers.back().type == marker.LINE_LIST) { - add_marker = false; - p.x = edge->m_vertex1.x; - p.y = edge->m_vertex1.y; - markers.markers.back().points.push_back(p); - p.x = edge->m_vertex2.x; - p.y = edge->m_vertex2.y; - markers.markers.back().points.push_back(p); - - } else { // otherwise create a new line list - - marker.type = marker.LINE_LIST; - marker.scale.x = 0.03; // 3cm wide lines - - p.x = edge->m_vertex1.x; - p.y = edge->m_vertex1.y; - marker.points.push_back(p); - p.x = edge->m_vertex2.x; - p.y = edge->m_vertex2.y; - marker.points.push_back(p); - } - - } break; - - default: // Unsupported shape - ROS_WARN_THROTTLE_NAMED(1.0, "DebugVis", "Unsupported Box2D shape %d", - static_cast(fixture->GetType())); - fixture = fixture->GetNext(); - continue; // Do not add broken marker - break; - } - - if (add_marker) { - markers.markers.push_back(marker); // Add the new marker - } - fixture = fixture->GetNext(); // Traverse the linked list of fixtures - } -} - -void DebugVisualization::Publish(const Timekeeper& timekeeper) { - // Iterate over the topics_ map as pair(name, topic) - - std::vector to_delete; - - for (auto& topic : topics_) { - if (!topic.second.needs_publishing) { - continue; - } - - // since if empty markers are published rviz will continue to publish - // using the old data, delete the topic list - if (topic.second.markers.markers.size() == 0) { - to_delete.push_back(topic.first); - } else { - // Iterate the marker array to update all the timestamps - for (unsigned int i = 0; i < topic.second.markers.markers.size(); i++) { - topic.second.markers.markers[i].header.stamp = timekeeper.GetSimTime(); - } - topic.second.publisher.publish(topic.second.markers); - topic.second.needs_publishing = false; - } - } - - if (to_delete.size() > 0) { - for (const auto& topic : to_delete) { - ROS_WARN_NAMED("DebugVis", "Deleting topic %s", topic.c_str()); - topics_.erase(topic); - } - PublishTopicList(); - } -} - -void DebugVisualization::VisualizeLayer(std::string name, Body* body) { - AddTopicIfNotExist(name); - - b2Fixture* fixture = body->physics_body_->GetFixtureList(); - - visualization_msgs::Marker marker; - if (fixture == NULL) return; // Nothing to visualize, empty linked list - - while (fixture != NULL) { // traverse fixture linked list - - marker.header.frame_id = "map"; - marker.id = topics_[name].markers.markers.size(); - marker.color.r = body->color_.r; - marker.color.g = body->color_.g; - marker.color.b = body->color_.b; - marker.color.a = body->color_.a; - marker.scale.x = marker.scale.y = marker.scale.z = 1.0; - marker.frame_locked = true; - marker.pose.position.x = body->physics_body_->GetPosition().x; - marker.pose.position.y = body->physics_body_->GetPosition().y; - - tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion - q.setRPY(0, 0, body->physics_body_ - ->GetAngle()); // from euler angles: roll, pitch, yaw - marker.pose.orientation = tf2::toMsg(q); - marker.type = marker.TRIANGLE_LIST; - - YamlReader reader(body->properties_); - YamlReader debug_reader = - reader.SubnodeOpt("debug", YamlReader::NodeTypeCheck::MAP); - float min_z = debug_reader.Get("min_z", 0.0); - float max_z = debug_reader.Get("max_z", 1.0); - - // Get the shape from the fixture - if (fixture->GetType() == b2Shape::e_edge) { - geometry_msgs::Point p; // b2Edge uses vertex1 and 2 for its edges - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); - - p.x = edge->m_vertex1.x; - p.y = edge->m_vertex1.y; - p.z = min_z; - marker.points.push_back(p); - p.x = edge->m_vertex2.x; - p.y = edge->m_vertex2.y; - p.z = min_z; - marker.points.push_back(p); - p.x = edge->m_vertex2.x; - p.y = edge->m_vertex2.y; - p.z = max_z; - marker.points.push_back(p); - - p.x = edge->m_vertex1.x; - p.y = edge->m_vertex1.y; - p.z = min_z; - marker.points.push_back(p); - p.x = edge->m_vertex2.x; - p.y = edge->m_vertex2.y; - p.z = max_z; - marker.points.push_back(p); - p.x = edge->m_vertex1.x; - p.y = edge->m_vertex1.y; - p.z = max_z; - marker.points.push_back(p); - } - - fixture = fixture->GetNext(); // Traverse the linked list of fixtures - } - - topics_[name].markers.markers.push_back(marker); // Add the new marker - topics_[name].needs_publishing = true; -} - -void DebugVisualization::Visualize(std::string name, b2Body* body, float r, - float g, float b, float a) { - AddTopicIfNotExist(name); - BodyToMarkers(topics_[name].markers, body, r, g, b, a); - topics_[name].needs_publishing = true; -} - -void DebugVisualization::Visualize(std::string name, b2Joint* joint, float r, - float g, float b, float a) { - AddTopicIfNotExist(name); - JointToMarkers(topics_[name].markers, joint, r, g, b, a); - topics_[name].needs_publishing = true; -} - -void DebugVisualization::Reset(std::string name) { - if (topics_.count(name) > 0) { // If the topic exists, clear it - topics_[name].markers.markers.clear(); - topics_[name].needs_publishing = true; - } -} - -void DebugVisualization::AddTopicIfNotExist(const std::string& name) { - // If the topic doesn't exist yet, create it - if (topics_.count(name) == 0) { - topics_[name] = { - node_.advertise(name, 0, true), true, - visualization_msgs::MarkerArray()}; - - ROS_INFO_ONCE_NAMED("DebugVis", "Visualizing %s", name.c_str()); - PublishTopicList(); - } -} - -void DebugVisualization::PublishTopicList() { - flatland_msgs::DebugTopicList topic_list; - for (auto const& topic_pair : topics_) - topic_list.topics.push_back(topic_pair.first); - topic_list_publisher_.publish(topic_list); -} -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name debug_visualization.cpp + * @brief Transform box2d types into published visualization messages + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/debug_visualization.h" +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +DebugVisualization::DebugVisualization() : node_("~debug") { + topic_list_publisher_ = + node_.advertise("topics", 0, true); +} + +DebugVisualization& DebugVisualization::Get() { + static DebugVisualization instance; + return instance; +} + +void DebugVisualization::JointToMarkers( + visualization_msgs::MarkerArray& markers, b2JointId joint, float r, + float g, float b, float a) { + b2JointType jtype = b2Joint_GetType(joint); + if (jtype == b2_distanceJoint || jtype == b2_mouseJoint) { + ROS_ERROR_NAMED("DebugVis", + "Unimplemented visualization joints. See b2World.cpp for " + "implementation"); + return; + } + + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.color.r = r; + marker.color.g = g; + marker.color.b = b; + marker.color.a = a; + marker.type = marker.LINE_LIST; + marker.scale.x = 0.01; + + b2BodyId bodyA = b2Joint_GetBodyA(joint); + b2BodyId bodyB = b2Joint_GetBodyB(joint); + b2Vec2 posA = b2Body_GetPosition(bodyA); + b2Vec2 posB = b2Body_GetPosition(bodyB); + + // Compute world anchor points from local anchors (weld joint only for now) + b2Vec2 localAnchorA = b2Joint_GetLocalAnchorA(joint); + b2Vec2 localAnchorB = b2Joint_GetLocalAnchorB(joint); + b2Vec2 worldAnchorA = b2TransformPoint(b2Body_GetTransform(bodyA), localAnchorA); + b2Vec2 worldAnchorB = b2TransformPoint(b2Body_GetTransform(bodyB), localAnchorB); + + geometry_msgs::Point p_bodyA, p_anchorA, p_bodyB, p_anchorB; + p_bodyA.x = posA.x; p_bodyA.y = posA.y; + p_anchorA.x = worldAnchorA.x; p_anchorA.y = worldAnchorA.y; + p_bodyB.x = posB.x; p_bodyB.y = posB.y; + p_anchorB.x = worldAnchorB.x; p_anchorB.y = worldAnchorB.y; + + // Lines: bodyA->anchorA, bodyB->anchorB, anchorA->anchorB + marker.id = markers.markers.size(); + marker.points.push_back(p_bodyA); + marker.points.push_back(p_anchorA); + marker.points.push_back(p_bodyB); + marker.points.push_back(p_anchorB); + marker.points.push_back(p_anchorA); + marker.points.push_back(p_anchorB); + + markers.markers.push_back(marker); + + marker.id = markers.markers.size(); + marker.type = marker.CUBE_LIST; + marker.scale.x = marker.scale.y = marker.scale.z = 0.03; + marker.points.clear(); + marker.points.push_back(p_anchorA); + marker.points.push_back(p_anchorB); + marker.points.push_back(p_bodyA); + marker.points.push_back(p_bodyB); + markers.markers.push_back(marker); +} + +void DebugVisualization::BodyToMarkers(visualization_msgs::MarkerArray& markers, + b2BodyId body, float r, float g, + float b, float a) { + int shape_count = b2Body_GetShapeCount(body); + if (shape_count == 0) return; + + std::vector shapes(shape_count); + b2Body_GetShapes(body, shapes.data(), shape_count); + + b2Vec2 pos = b2Body_GetPosition(body); + float angle = b2Rot_GetAngle(b2Body_GetRotation(body)); + + for (int si = 0; si < shape_count; si++) { + b2ShapeId shape = shapes[si]; + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.id = markers.markers.size(); + marker.color.r = r; + marker.color.g = g; + marker.color.b = b; + marker.color.a = a; + marker.pose.position.x = pos.x; + marker.pose.position.y = pos.y; + tf2::Quaternion q; + q.setRPY(0, 0, angle); + marker.pose.orientation = tf2::toMsg(q); + bool add_marker = true; + + b2ShapeType stype = b2Shape_GetType(shape); + if (stype == b2_circleShape) { + b2Circle circle = b2Shape_GetCircle(shape); + + marker.type = marker.SPHERE_LIST; + float diameter = circle.radius * 2.0f; + marker.scale.z = 0.01; + marker.scale.x = diameter; + marker.scale.y = diameter; + + geometry_msgs::Point p; + p.x = circle.center.x; + p.y = circle.center.y; + marker.points.push_back(p); + + } else if (stype == b2_polygonShape) { + b2Polygon poly = b2Shape_GetPolygon(shape); + marker.type = marker.LINE_STRIP; + marker.scale.x = 0.03; + + for (int i = 0; i < poly.count; i++) { + geometry_msgs::Point p; + p.x = poly.vertices[i].x; + p.y = poly.vertices[i].y; + marker.points.push_back(p); + } + if (poly.count > 0) marker.points.push_back(marker.points[0]); + + } else if (stype == b2_segmentShape) { + b2Segment seg = b2Shape_GetSegment(shape); + geometry_msgs::Point p; + + // If the last marker is a line list, extend it + if (markers.markers.size() > 0 && + markers.markers.back().type == marker.LINE_LIST) { + add_marker = false; + p.x = seg.point1.x; p.y = seg.point1.y; + markers.markers.back().points.push_back(p); + p.x = seg.point2.x; p.y = seg.point2.y; + markers.markers.back().points.push_back(p); + } else { + marker.type = marker.LINE_LIST; + marker.scale.x = 0.03; + p.x = seg.point1.x; p.y = seg.point1.y; + marker.points.push_back(p); + p.x = seg.point2.x; p.y = seg.point2.y; + marker.points.push_back(p); + } + + } else { + ROS_WARN_THROTTLE_NAMED(1.0, "DebugVis", "Unsupported Box2D shape type %d", + static_cast(stype)); + continue; + } + + if (add_marker) { + markers.markers.push_back(marker); + } + } +} + +void DebugVisualization::Publish(const Timekeeper& timekeeper) { + // Iterate over the topics_ map as pair(name, topic) + + std::vector to_delete; + + for (auto& topic : topics_) { + if (!topic.second.needs_publishing) { + continue; + } + + // since if empty markers are published rviz will continue to publish + // using the old data, delete the topic list + if (topic.second.markers.markers.size() == 0) { + to_delete.push_back(topic.first); + } else { + // Iterate the marker array to update all the timestamps + for (unsigned int i = 0; i < topic.second.markers.markers.size(); i++) { + topic.second.markers.markers[i].header.stamp = timekeeper.GetSimTime(); + } + topic.second.publisher.publish(topic.second.markers); + topic.second.needs_publishing = false; + } + } + + if (to_delete.size() > 0) { + for (const auto& topic : to_delete) { + ROS_WARN_NAMED("DebugVis", "Deleting topic %s", topic.c_str()); + topics_.erase(topic); + } + PublishTopicList(); + } +} + +void DebugVisualization::VisualizeLayer(std::string name, Body* body) { + AddTopicIfNotExist(name); + + int shape_count = b2Body_GetShapeCount(body->physics_body_); + + visualization_msgs::Marker marker; + if (shape_count == 0) return; + + std::vector shapes(shape_count); + b2Body_GetShapes(body->physics_body_, shapes.data(), shape_count); + + for (int si = 0; si < shape_count; si++) { + b2ShapeId shape = shapes[si]; + if (b2Shape_GetType(shape) != b2_segmentShape) continue; // layer only has segments + b2Segment seg = b2Shape_GetSegment(shape); + + marker.header.frame_id = "map"; + marker.id = topics_[name].markers.markers.size(); + marker.color.r = body->color_.r; + marker.color.g = body->color_.g; + marker.color.b = body->color_.b; + marker.color.a = body->color_.a; + marker.scale.x = marker.scale.y = marker.scale.z = 1.0; + marker.frame_locked = true; + marker.pose.position.x = b2Body_GetPosition(body->physics_body_).x; + marker.pose.position.y = b2Body_GetPosition(body->physics_body_).y; + + tf2::Quaternion q; + q.setRPY(0, 0, b2Rot_GetAngle(b2Body_GetRotation(body->physics_body_))); + marker.pose.orientation = tf2::toMsg(q); + marker.type = marker.TRIANGLE_LIST; + marker.points.clear(); + + YamlReader reader(body->properties_); + YamlReader debug_reader = + reader.SubnodeOpt("debug", YamlReader::NodeTypeCheck::MAP); + float min_z = debug_reader.Get("min_z", 0.0); + float max_z = debug_reader.Get("max_z", 1.0); + + geometry_msgs::Point p; + p.x = seg.point1.x; p.y = seg.point1.y; p.z = min_z; + marker.points.push_back(p); + p.x = seg.point2.x; p.y = seg.point2.y; p.z = min_z; + marker.points.push_back(p); + p.x = seg.point2.x; p.y = seg.point2.y; p.z = max_z; + marker.points.push_back(p); + p.x = seg.point1.x; p.y = seg.point1.y; p.z = min_z; + marker.points.push_back(p); + p.x = seg.point2.x; p.y = seg.point2.y; p.z = max_z; + marker.points.push_back(p); + p.x = seg.point1.x; p.y = seg.point1.y; p.z = max_z; + marker.points.push_back(p); + + topics_[name].markers.markers.push_back(marker); + } + topics_[name].needs_publishing = true; +} + +void DebugVisualization::Visualize(std::string name, b2BodyId body, float r, + float g, float b, float a) { + AddTopicIfNotExist(name); + BodyToMarkers(topics_[name].markers, body, r, g, b, a); + topics_[name].needs_publishing = true; +} + +void DebugVisualization::Visualize(std::string name, b2JointId joint, float r, + float g, float b, float a) { + AddTopicIfNotExist(name); + JointToMarkers(topics_[name].markers, joint, r, g, b, a); + topics_[name].needs_publishing = true; +} + +void DebugVisualization::Reset(std::string name) { + if (topics_.count(name) > 0) { // If the topic exists, clear it + topics_[name].markers.markers.clear(); + topics_[name].needs_publishing = true; + } +} + +void DebugVisualization::AddTopicIfNotExist(const std::string& name) { + // If the topic doesn't exist yet, create it + if (topics_.count(name) == 0) { + topics_[name] = { + node_.advertise(name, 0, true), true, + visualization_msgs::MarkerArray()}; + + ROS_INFO_ONCE_NAMED("DebugVis", "Visualizing %s", name.c_str()); + PublishTopicList(); + } +} + +void DebugVisualization::PublishTopicList() { + flatland_msgs::DebugTopicList topic_list; + for (auto const& topic_pair : topics_) + topic_list.topics.push_back(topic_pair.first); + topic_list_publisher_.publish(topic_list); +} +}; // namespace flatland_server diff --git a/flatland_server/src/dummy_model_plugin.cpp b/flatland_server/src/dummy_model_plugin.cpp index 989eb6ec..1c7b848d 100644 --- a/flatland_server/src/dummy_model_plugin.cpp +++ b/flatland_server/src/dummy_model_plugin.cpp @@ -1,82 +1,82 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name dummy_model_plugin.cpp - * @brief Dummy model plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -using namespace flatland_server; - -namespace flatland_plugins { - -void DummyModelPlugin::OnInitialize(const YAML::Node &config) { - dummy_param_float_ = config["dummy_param_float"].as(); - dummy_param_string_ = config["dummy_param_string"].as(); - dummy_param_int_ = config["dummy_param_int"].as(); - - // Dummy plugin has the these values enforced for testing - if (fabs(dummy_param_float_ - 0.123456) > 1e-7) { - throw YAMLException( - "dummy_param_float must be 0.1253456, instead it was \"" + - std::to_string(dummy_param_float_) + "\""); - } - - if (dummy_param_int_ != 123456) { - throw YAMLException("dummy_param_int must be 1253456, instead it was \"" + - std::to_string(dummy_param_int_) + "\""); - } - - if (dummy_param_string_ != "dummy_test_123456") { - throw YAMLException( - "dummy_param_float must be dummy_test_123456, instead it was \"" + - dummy_param_string_ + "\""); - } -} -}; - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyModelPlugin, - flatland_server::ModelPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name dummy_model_plugin.cpp + * @brief Dummy model plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +using namespace flatland_server; + +namespace flatland_plugins { + +void DummyModelPlugin::OnInitialize(const YAML::Node &config) { + dummy_param_float_ = config["dummy_param_float"].as(); + dummy_param_string_ = config["dummy_param_string"].as(); + dummy_param_int_ = config["dummy_param_int"].as(); + + // Dummy plugin has the these values enforced for testing + if (fabs(dummy_param_float_ - 0.123456) > 1e-7) { + throw YAMLException( + "dummy_param_float must be 0.1253456, instead it was \"" + + std::to_string(dummy_param_float_) + "\""); + } + + if (dummy_param_int_ != 123456) { + throw YAMLException("dummy_param_int must be 1253456, instead it was \"" + + std::to_string(dummy_param_int_) + "\""); + } + + if (dummy_param_string_ != "dummy_test_123456") { + throw YAMLException( + "dummy_param_float must be dummy_test_123456, instead it was \"" + + dummy_param_string_ + "\""); + } +} +}; + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyModelPlugin, + flatland_server::ModelPlugin) diff --git a/flatland_server/src/dummy_world_plugin.cpp b/flatland_server/src/dummy_world_plugin.cpp index 307cc749..2c352aab 100644 --- a/flatland_server/src/dummy_world_plugin.cpp +++ b/flatland_server/src/dummy_world_plugin.cpp @@ -1,77 +1,77 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name dummy_world_plugin.cpp - * @brief Dummy world plugin - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -using namespace flatland_server; - -namespace flatland_plugins { - -void DummyWorldPlugin::OnInitialize(const YAML::Node &config) { - if (world_ != NULL) { - throw PluginException("World is not NULL!"); - } - if (name_ != "DummyWorldPluginName") { - throw PluginException( - "Dummy world plugin name is in correct, instead of " - "\"DummyWorldPluginName\", the name is " + - name_); - } - if (type_ != "DummyWorldPluginType") { - throw PluginException( - "Dummy world plugin type is in correct, instead of " - "\"DummyWorldPluginType\", the type is " + - type_); - } -} -}; - -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyWorldPlugin, - flatland_server::WorldPlugin) \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name dummy_world_plugin.cpp + * @brief Dummy world plugin + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +using namespace flatland_server; + +namespace flatland_plugins { + +void DummyWorldPlugin::OnInitialize(const YAML::Node &config) { + if (world_ != NULL) { + throw PluginException("World is not NULL!"); + } + if (name_ != "DummyWorldPluginName") { + throw PluginException( + "Dummy world plugin name is in correct, instead of " + "\"DummyWorldPluginName\", the name is " + + name_); + } + if (type_ != "DummyWorldPluginType") { + throw PluginException( + "Dummy world plugin type is in correct, instead of " + "\"DummyWorldPluginType\", the type is " + + type_); + } +} +}; + +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyWorldPlugin, + flatland_server::WorldPlugin) diff --git a/flatland_server/src/entity.cpp b/flatland_server/src/entity.cpp index 7531f5d3..9cb43dd8 100644 --- a/flatland_server/src/entity.cpp +++ b/flatland_server/src/entity.cpp @@ -1,59 +1,59 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name entity.cpp - * @brief implements flatland entity - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -namespace flatland_server { - -Entity::Entity(b2World *physics_world, const std::string &name) - : physics_world_(physics_world), name_(name) {} - -const std::string &Entity::GetName() const { return name_; } - -b2World *Entity::GetPhysicsWorld() { return physics_world_; } - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name entity.cpp + * @brief implements flatland entity + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +namespace flatland_server { + +Entity::Entity(b2WorldId physics_world, const std::string &name) + : physics_world_(physics_world), name_(name) {} + +const std::string &Entity::GetName() const { return name_; } + +b2WorldId Entity::GetPhysicsWorld() { return physics_world_; } + +}; // namespace flatland_server diff --git a/flatland_server/src/flatland_benchmark.cpp b/flatland_server/src/flatland_benchmark.cpp new file mode 100644 index 00000000..3b74f475 --- /dev/null +++ b/flatland_server/src/flatland_benchmark.cpp @@ -0,0 +1,136 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2024 Avidbots Corp. + * @name flatland_benchmark.cpp + * @brief Benchmark version of flatland_server + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "flatland_server/simulation_manager.h" + +/** Global variables */ +flatland_server::SimulationManager *simulation_manager; +double benchmark_duration = 10.0; // overwritten by rosparam "benchmark_duration" if set +double benchmark_start = 0.0f; + +/** + * @name SigintHandler + * @brief Interrupt handler - sends shutdown signal to simulation_manager + * @param[in] sig: signal itself + */ +void SigintHandler(int sig) { + ROS_WARN_NAMED("Benchmark", "*** Shutting down... ***"); + + if (simulation_manager != nullptr) { + simulation_manager->Shutdown(); + delete simulation_manager; + simulation_manager = nullptr; + } + ROS_INFO_STREAM_NAMED("Benchmark", "Beginning ros shutdown"); + ros::shutdown(); +} + +void CheckTimeout(const ros::WallTimerEvent& timer_event) { + double elapsed = simulation_manager->timekeeper_.GetSimTime().toSec(); + if (elapsed >= benchmark_duration) { + double bench_time = ros::WallTime::now().toSec() - benchmark_start; + ROS_INFO_NAMED("Benchmark", "Benchmark complete. Ran %ld iterations in %.3f sec; %.3f iter/sec. Shutting down.", + simulation_manager->iterations_, bench_time, (double)(simulation_manager->iterations_)/bench_time); + ros::shutdown(); + } + ROS_INFO_THROTTLE_NAMED(1.0, "Benchmark", "Elapsed: %.1f/%.1f.", elapsed, benchmark_duration); +} + +/** + * @name main + * @brief Entrypoint for Flatland Server ros node + */ +int main(int argc, char **argv) { + ros::init(argc, argv, "flatland", ros::init_options::NoSigintHandler); + ros::NodeHandle node_handle("~"); + + // todo: Load default parameters, run on a specific (default incl.) map for N seconds, then report. + + // Load parameters + std::string world_path; // The file path to the world.yaml file + if (!node_handle.getParam("world_path", world_path)) { + ROS_FATAL_NAMED("Benchmark", "No world_path parameter given!"); + ros::shutdown(); + return 1; + } + + float update_rate = 200.0; // The physics update rate (Hz) + node_handle.getParam("update_rate", update_rate); + + float step_size = 1 / 200.0; + node_handle.getParam("step_size", step_size); + + bool show_viz = false; + node_handle.getParam("show_viz", show_viz); + + float viz_pub_rate = 30.0; + node_handle.getParam("viz_pub_rate", viz_pub_rate); + + node_handle.getParam("benchmark_duration", benchmark_duration); + + // Create simulation manager object + simulation_manager = new flatland_server::SimulationManager( + world_path, update_rate, step_size, show_viz, viz_pub_rate); + + // Register sigint shutdown handler + signal(SIGINT, SigintHandler); + + // timeout + ros::WallTimer timeout_timer = node_handle.createWallTimer(ros::WallDuration(0.01), &CheckTimeout); + + ROS_INFO_STREAM_NAMED("Benchmark", "Initialized"); + benchmark_start = ros::WallTime::now().toSec(); + simulation_manager->Main(/*benchmark=*/true); + + ROS_INFO_STREAM_NAMED("Benchmark", "Returned from simulation manager main"); + delete simulation_manager; + simulation_manager = nullptr; + return 0; +} diff --git a/flatland_server/src/flatland_server_node.cpp b/flatland_server/src/flatland_server_node.cpp index 871b666b..261f78da 100644 --- a/flatland_server/src/flatland_server_node.cpp +++ b/flatland_server/src/flatland_server_node.cpp @@ -1,115 +1,130 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_server_ndoe.cpp - * @brief Load params and run the ros node for flatland_server - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -#include "flatland_server/simulation_manager.h" - -/** Global variables */ -flatland_server::SimulationManager *simulation_manager; - -/** - * @name SigintHandler - * @brief Interrupt handler - sends shutdown signal to simulation_manager - * @param[in] sig: signal itself - */ -void SigintHandler(int sig) { - ROS_WARN_NAMED("Node", "*** Shutting down... ***"); - - if (simulation_manager != nullptr) { - simulation_manager->Shutdown(); - delete simulation_manager; - simulation_manager = nullptr; - } - ROS_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); - ros::shutdown(); -} - -/** - * @name main - * @brief Entrypoint for Flatland Server ros node - */ -int main(int argc, char **argv) { - ros::init(argc, argv, "flatland", ros::init_options::NoSigintHandler); - ros::NodeHandle node_handle("~"); - - // Load parameters - std::string world_path; // The file path to the world.yaml file - if (!node_handle.getParam("world_path", world_path)) { - ROS_FATAL_NAMED("Node", "No world_path parameter given!"); - ros::shutdown(); - return 1; - } - - float update_rate = 200.0; // The physics update rate (Hz) - node_handle.getParam("update_rate", update_rate); - - float step_size = 1 / 200.0; - node_handle.getParam("step_size", step_size); - - bool show_viz = false; - node_handle.getParam("show_viz", show_viz); - - float viz_pub_rate = 30.0; - node_handle.getParam("viz_pub_rate", viz_pub_rate); - - // Create simulation manager object - simulation_manager = new flatland_server::SimulationManager( - world_path, update_rate, step_size, show_viz, viz_pub_rate); - - // Register sigint shutdown handler - signal(SIGINT, SigintHandler); - - ROS_INFO_STREAM_NAMED("Node", "Initialized"); - simulation_manager->Main(); - - ROS_INFO_STREAM_NAMED("Node", "Returned from simulation manager main"); - delete simulation_manager; - simulation_manager = nullptr; - return 0; -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_server_ndoe.cpp + * @brief Load params and run the ros node for flatland_server + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "flatland_server/simulation_manager.h" + +/** Global variables */ +flatland_server::SimulationManager *simulation_manager; + +/** + * @name SigintHandler + * @brief Interrupt handler - sends shutdown signal to simulation_manager + * @param[in] sig: signal itself + */ +void SigintHandler(int sig) { + ROS_WARN_NAMED("Node", "*** Shutting down... ***"); + + if (simulation_manager != nullptr) { + simulation_manager->Shutdown(); + delete simulation_manager; + simulation_manager = nullptr; + } + ROS_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); + ros::shutdown(); +} + +/** + * @name main + * @brief Entrypoint for Flatland Server ros node + */ +int main(int argc, char **argv) { + ros::init(argc, argv, "flatland", ros::init_options::NoSigintHandler); + ros::NodeHandle node_handle("~"); + + // Load parameters + std::string world_path; // The file path to the world.yaml file + if (!node_handle.getParam("world_path", world_path)) { + ROS_FATAL_NAMED("Node", "No world_path parameter given!"); + ros::shutdown(); + return 1; + } + + std::string models_path; + if (!node_handle.getParam("models_path", models_path)) { + ROS_FATAL_NAMED("Node", "No models_path parameter given!"); + ros::shutdown(); + return 1; + } + + std::string world_plugins_path; + if (!node_handle.getParam("world_plugins_path", world_plugins_path)) { + ROS_FATAL_NAMED("Node", "No world_plugins_path parameter given!"); + ros::shutdown(); + return 1; + } + + float update_rate = 200.0; // The physics update rate (Hz) + node_handle.getParam("update_rate", update_rate); + + float step_size = 1 / 200.0; + node_handle.getParam("step_size", step_size); + + bool show_viz = false; + node_handle.getParam("show_viz", show_viz); + + float viz_pub_rate = 30.0; + node_handle.getParam("viz_pub_rate", viz_pub_rate); + + // Create simulation manager object + simulation_manager = new flatland_server::SimulationManager( + world_path, models_path, world_plugins_path, update_rate, step_size, + show_viz, viz_pub_rate); + + // Register sigint shutdown handler + signal(SIGINT, SigintHandler); + + ROS_INFO_STREAM_NAMED("Node", "Initialized"); + simulation_manager->Main(); + + ROS_INFO_STREAM_NAMED("Node", "Returned from simulation manager main"); + delete simulation_manager; + simulation_manager = nullptr; + return 0; +} diff --git a/flatland_server/src/geometry.cpp b/flatland_server/src/geometry.cpp index 7d877c25..3174b22c 100644 --- a/flatland_server/src/geometry.cpp +++ b/flatland_server/src/geometry.cpp @@ -1,88 +1,88 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name geometry.cpp - * @brief Geometry functions - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/geometry.h" -#include -#include - -namespace flatland_server { - -/** - * @brief Return a RotateTranslate given translation and rotation - * - * @param dx X translation - * @param dy Y translation - * @param a rotation (radians) - * - * @return THe RotateTranslate object - */ -RotateTranslate Geometry::CreateTransform(double dx, double dy, double a) { - RotateTranslate out = {dx, dy, cosf(a), sinf(a)}; - return out; -} - -/** - * @param Transform a b2Vec2 copy by a RotateTranslate object - * - * @param in The input vector - * @param rt The transformation - * - * @return - */ -b2Vec2 Geometry::Transform(const b2Vec2& in, const RotateTranslate& rt) { - b2Vec2 out; - out.x = in.x * rt.cos - in.y * rt.sin + rt.dx; - out.y = in.x * rt.sin + in.y * rt.cos + rt.dy; - return out; -} - -b2Vec2 Geometry::InverseTransform(const b2Vec2& in, const RotateTranslate& rt) { - b2Vec2 out; - out.x = (in.x - rt.dx) * rt.cos + (in.y - rt.dy) * rt.sin; - out.y = -(in.x - rt.dx) * rt.sin + (in.y - rt.dy) * rt.cos; - return out; -} -}; +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name geometry.cpp + * @brief Geometry functions + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/geometry.h" +#include +#include + +namespace flatland_server { + +/** + * @brief Return a RotateTranslate given translation and rotation + * + * @param dx X translation + * @param dy Y translation + * @param a rotation (radians) + * + * @return THe RotateTranslate object + */ +RotateTranslate Geometry::CreateTransform(double dx, double dy, double a) { + RotateTranslate out = {dx, dy, cosf(a), sinf(a)}; + return out; +} + +/** + * @param Transform a b2Vec2 copy by a RotateTranslate object + * + * @param in The input vector + * @param rt The transformation + * + * @return + */ +b2Vec2 Geometry::Transform(const b2Vec2& in, const RotateTranslate& rt) { + b2Vec2 out; + out.x = in.x * rt.cos - in.y * rt.sin + rt.dx; + out.y = in.x * rt.sin + in.y * rt.cos + rt.dy; + return out; +} + +b2Vec2 Geometry::InverseTransform(const b2Vec2& in, const RotateTranslate& rt) { + b2Vec2 out; + out.x = (in.x - rt.dx) * rt.cos + (in.y - rt.dy) * rt.sin; + out.y = -(in.x - rt.dx) * rt.sin + (in.y - rt.dy) * rt.cos; + return out; +} +}; diff --git a/flatland_server/src/interactive_marker_manager.cpp b/flatland_server/src/interactive_marker_manager.cpp index 04520f2f..eeab6b08 100644 --- a/flatland_server/src/interactive_marker_manager.cpp +++ b/flatland_server/src/interactive_marker_manager.cpp @@ -1,248 +1,247 @@ -#include - -namespace flatland_server { - -InteractiveMarkerManager::InteractiveMarkerManager( - std::vector *model_list_ptr, PluginManager *plugin_manager_ptr) { - models_ = model_list_ptr; - plugin_manager_ = plugin_manager_ptr; - manipulating_model_ = false; - - // Initialize interactive marker server - interactive_marker_server_.reset( - new interactive_markers::InteractiveMarkerServer( - "interactive_model_markers")); - - // Add "Delete Model" context menu option to menu handler and bind callback - menu_handler_.setCheckState( - menu_handler_.insert( - "Delete Model", - boost::bind(&InteractiveMarkerManager::deleteModelMenuCallback, this, - _1)), - interactive_markers::MenuHandler::NO_CHECKBOX); - interactive_marker_server_->applyChanges(); -} - -void InteractiveMarkerManager::createInteractiveMarker( - const std::string &model_name, const Pose &pose, - const visualization_msgs::MarkerArray &body_markers) { - // Set up interactive marker control objects to allow both translation and - // rotation movement - visualization_msgs::InteractiveMarkerControl plane_control; - plane_control.always_visible = true; - plane_control.orientation.w = 0.707; - plane_control.orientation.y = 0.707; - plane_control.name = "move_xy"; - plane_control.interaction_mode = - visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; - visualization_msgs::InteractiveMarkerControl rotate_control; - rotate_control.always_visible = true; - rotate_control.orientation.w = 0.707; - rotate_control.orientation.y = 0.707; - rotate_control.name = "rotate_z"; - rotate_control.interaction_mode = - visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; - - // Add a non-interactive text marker with the model name - visualization_msgs::InteractiveMarkerControl no_control; - no_control.always_visible = true; - no_control.name = "no_control"; - no_control.interaction_mode = - visualization_msgs::InteractiveMarkerControl::NONE; - visualization_msgs::Marker text_marker; - text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - text_marker.pose.position.x = 0.5; - text_marker.pose.position.y = 0.0; - text_marker.scale.z = 0.5; - text_marker.text = model_name; - no_control.markers.push_back(text_marker); - - // Add a cube marker to be an easy-to-manipulate target in Rviz - visualization_msgs::Marker easy_to_click_cube; - easy_to_click_cube.type = visualization_msgs::Marker::CUBE; - easy_to_click_cube.color.r = 0.0; - easy_to_click_cube.color.g = 1.0; - easy_to_click_cube.color.b = 0.0; - easy_to_click_cube.color.a = 0.5; - easy_to_click_cube.scale.x = 0.5; - easy_to_click_cube.scale.y = 0.5; - easy_to_click_cube.scale.z = 0.05; - easy_to_click_cube.pose.position.x = 0.0; - plane_control.markers.push_back(easy_to_click_cube); - - // Also add body markers to the no_control object to visualize model pose - // while moving its interactive marker - for (size_t i = 0; i < body_markers.markers.size(); i++) { - visualization_msgs::Marker transformed_body_marker = - body_markers.markers[i]; - - // Transform original body frame marker from global to local frame - RotateTranslate rt = Geometry::CreateTransform(pose.x, pose.y, pose.theta); - transformed_body_marker.header.frame_id = ""; - transformed_body_marker.header.stamp = ros::Time(0); - transformed_body_marker.pose.position.x = - (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos + - (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin; - transformed_body_marker.pose.position.y = - -(body_markers.markers[i].pose.position.x - rt.dx) * rt.sin + - (body_markers.markers[i].pose.position.y - rt.dy) * rt.cos; - transformed_body_marker.pose.orientation.w = 1.0; - transformed_body_marker.pose.orientation.x = 0.0; - transformed_body_marker.pose.orientation.y = 0.0; - transformed_body_marker.pose.orientation.z = 0.0; - - // Make line strips thicker than the original - if (transformed_body_marker.type == - visualization_msgs::Marker::LINE_STRIP || - transformed_body_marker.type == visualization_msgs::Marker::LINE_LIST) { - transformed_body_marker.scale.x = 0.1; - } - - // Add transformed body marker to interactive marker control object - no_control.markers.push_back(transformed_body_marker); - } - - // Send new interactive marker to server - visualization_msgs::InteractiveMarker new_interactive_marker; - new_interactive_marker.header.frame_id = "map"; - new_interactive_marker.header.stamp = ros::Time(0); - new_interactive_marker.name = model_name; - new_interactive_marker.pose.position.x = pose.x; - new_interactive_marker.pose.position.y = pose.y; - new_interactive_marker.pose.orientation.w = cos(0.5 * pose.theta); - new_interactive_marker.pose.orientation.z = sin(0.5 * pose.theta); - new_interactive_marker.controls.push_back(plane_control); - new_interactive_marker.controls.push_back(rotate_control); - new_interactive_marker.controls.push_back(no_control); - interactive_marker_server_->insert(new_interactive_marker); - - // Bind feedback callbacks for the new interactive marker - interactive_marker_server_->setCallback( - model_name, - boost::bind(&InteractiveMarkerManager::processMouseUpFeedback, this, _1), - visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP); - interactive_marker_server_->setCallback( - model_name, - boost::bind(&InteractiveMarkerManager::processMouseDownFeedback, this, - _1), - visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN); - interactive_marker_server_->setCallback( - model_name, - boost::bind(&InteractiveMarkerManager::processPoseUpdateFeedback, this, - _1), - visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE); - - // Add context menu to the new interactive marker - menu_handler_.apply(*interactive_marker_server_, model_name); - - // Apply changes to server - interactive_marker_server_->applyChanges(); -} - -void InteractiveMarkerManager::deleteModelMenuCallback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { - // Delete the model just as when the DeleteModel service is called - for (unsigned int i = 0; i < (*models_).size(); i++) { - if ((*models_)[i]->GetName() == feedback->marker_name) { - // delete the plugins associated with the model - plugin_manager_->DeleteModelPlugin((*models_)[i]); - delete (*models_)[i]; - (*models_).erase((*models_).begin() + i); - - // Also remove corresponding interactive marker - deleteInteractiveMarker(feedback->marker_name); - break; - } - } - - // Update menu handler and server - menu_handler_.apply(*interactive_marker_server_, feedback->marker_name); - interactive_marker_server_->applyChanges(); -} - -void InteractiveMarkerManager::deleteInteractiveMarker( - const std::string &model_name) { - // Remove target interactive marker by name and - // update the server - interactive_marker_server_->erase(model_name); - interactive_marker_server_->applyChanges(); -} - -void InteractiveMarkerManager::processMouseUpFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { - // Update model that was manipulated the same way - // as when the MoveModel service is called - for (unsigned int i = 0; i < models_->size(); i++) { - if ((*models_)[i]->GetName() == feedback->marker_name) { - Pose new_pose; - new_pose.x = feedback->pose.position.x; - new_pose.y = feedback->pose.position.y; - new_pose.theta = atan2( - 2.0 * feedback->pose.orientation.w * feedback->pose.orientation.z, - 1.0 - - 2.0 * feedback->pose.orientation.z * - feedback->pose.orientation.z); - (*models_)[i]->SetPose(new_pose); - break; - } - } - manipulating_model_ = false; - interactive_marker_server_->applyChanges(); -} - -void InteractiveMarkerManager::processMouseDownFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { - manipulating_model_ = true; -} - -void InteractiveMarkerManager::processPoseUpdateFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { - pose_update_stamp_ = ros::WallTime::now(); -} - -void InteractiveMarkerManager::update() { - // Loop through each model, extract the pose of the root body, - // and use it to update the interactive marker pose. Only - // necessary to compute if user is not currently dragging - // an interactive marker - if (!manipulating_model_) { - for (size_t i = 0; i < (*models_).size(); i++) { - geometry_msgs::Pose new_pose; - new_pose.position.x = - (*models_)[i]->bodies_[0]->physics_body_->GetPosition().x; - new_pose.position.y = - (*models_)[i]->bodies_[0]->physics_body_->GetPosition().y; - double theta = (*models_)[i]->bodies_[0]->physics_body_->GetAngle(); - new_pose.orientation.w = cos(0.5 * theta); - new_pose.orientation.z = sin(0.5 * theta); - interactive_marker_server_->setPose((*models_)[i]->GetName(), new_pose); - interactive_marker_server_->applyChanges(); - } - } - - // Detect when interaction stops without triggering a MOUSE_UP event by - // monitoring the time since the last pose update feedback, which comes - // in at 33 Hz if the user is dragging the marker. When the stream of - // pose update feedback stops, automatically clear the manipulating_model_ - // flag to unpause the simulation. - double dt = 0; - try { - dt = (ros::WallTime::now() - pose_update_stamp_).toSec(); - } catch (std::runtime_error &ex) { - ROS_ERROR( - "Flatland Interactive Marker Manager runtime error: (%f - %f) [%s]", - ros::WallTime::now().toSec(), pose_update_stamp_.toSec(), ex.what()); - } - if (manipulating_model_ && dt > 0.1 && dt < 1.0) { - manipulating_model_ = false; - } -} - -InteractiveMarkerManager::~InteractiveMarkerManager() { - interactive_marker_server_.reset(); -} -} +#include + +namespace flatland_server { + +InteractiveMarkerManager::InteractiveMarkerManager( + std::vector *model_list_ptr, PluginManager *plugin_manager_ptr) { + models_ = model_list_ptr; + plugin_manager_ = plugin_manager_ptr; + manipulating_model_ = false; + + // Initialize interactive marker server + interactive_marker_server_.reset( + new interactive_markers::InteractiveMarkerServer( + "interactive_model_markers")); + + // Add "Delete Model" context menu option to menu handler and bind callback + menu_handler_.setCheckState( + menu_handler_.insert( + "Delete Model", + boost::bind(&InteractiveMarkerManager::deleteModelMenuCallback, this, + _1)), + interactive_markers::MenuHandler::NO_CHECKBOX); + interactive_marker_server_->applyChanges(); +} + +void InteractiveMarkerManager::createInteractiveMarker( + const std::string &model_name, const Pose &pose, + const visualization_msgs::MarkerArray &body_markers) { + // Set up interactive marker control objects to allow both translation and + // rotation movement + visualization_msgs::InteractiveMarkerControl plane_control; + plane_control.always_visible = true; + plane_control.orientation.w = 0.707; + plane_control.orientation.y = 0.707; + plane_control.name = "move_xy"; + plane_control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + visualization_msgs::InteractiveMarkerControl rotate_control; + rotate_control.always_visible = true; + rotate_control.orientation.w = 0.707; + rotate_control.orientation.y = 0.707; + rotate_control.name = "rotate_z"; + rotate_control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; + + // Add a non-interactive text marker with the model name + visualization_msgs::InteractiveMarkerControl no_control; + no_control.always_visible = true; + no_control.name = "no_control"; + no_control.interaction_mode = + visualization_msgs::InteractiveMarkerControl::NONE; + visualization_msgs::Marker text_marker; + text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.pose.position.x = 0.5; + text_marker.pose.position.y = 0.0; + text_marker.scale.z = 0.5; + text_marker.text = model_name; + no_control.markers.push_back(text_marker); + + // Add a cube marker to be an easy-to-manipulate target in Rviz + visualization_msgs::Marker easy_to_click_cube; + easy_to_click_cube.type = visualization_msgs::Marker::CUBE; + easy_to_click_cube.color.r = 0.0; + easy_to_click_cube.color.g = 1.0; + easy_to_click_cube.color.b = 0.0; + easy_to_click_cube.color.a = 0.5; + easy_to_click_cube.scale.x = 0.5; + easy_to_click_cube.scale.y = 0.5; + easy_to_click_cube.scale.z = 0.05; + easy_to_click_cube.pose.position.x = 0.0; + plane_control.markers.push_back(easy_to_click_cube); + + // Also add body markers to the no_control object to visualize model pose + // while moving its interactive marker + for (size_t i = 0; i < body_markers.markers.size(); i++) { + visualization_msgs::Marker transformed_body_marker = + body_markers.markers[i]; + + // Transform original body frame marker from global to local frame + RotateTranslate rt = Geometry::CreateTransform(pose.x, pose.y, pose.theta); + transformed_body_marker.header.frame_id = ""; + transformed_body_marker.header.stamp = ros::Time(0); + transformed_body_marker.pose.position.x = + (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos + + (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin; + transformed_body_marker.pose.position.y = + -(body_markers.markers[i].pose.position.x - rt.dx) * rt.sin + + (body_markers.markers[i].pose.position.y - rt.dy) * rt.cos; + transformed_body_marker.pose.orientation.w = 1.0; + transformed_body_marker.pose.orientation.x = 0.0; + transformed_body_marker.pose.orientation.y = 0.0; + transformed_body_marker.pose.orientation.z = 0.0; + + // Make line strips thicker than the original + if (transformed_body_marker.type == + visualization_msgs::Marker::LINE_STRIP || + transformed_body_marker.type == visualization_msgs::Marker::LINE_LIST) { + transformed_body_marker.scale.x = 0.1; + } + + // Add transformed body marker to interactive marker control object + no_control.markers.push_back(transformed_body_marker); + } + + // Send new interactive marker to server + visualization_msgs::InteractiveMarker new_interactive_marker; + new_interactive_marker.header.frame_id = "map"; + new_interactive_marker.header.stamp = ros::Time(0); + new_interactive_marker.name = model_name; + new_interactive_marker.pose.position.x = pose.x; + new_interactive_marker.pose.position.y = pose.y; + new_interactive_marker.pose.orientation.w = cos(0.5 * pose.theta); + new_interactive_marker.pose.orientation.z = sin(0.5 * pose.theta); + new_interactive_marker.controls.push_back(plane_control); + new_interactive_marker.controls.push_back(rotate_control); + new_interactive_marker.controls.push_back(no_control); + interactive_marker_server_->insert(new_interactive_marker); + + // Bind feedback callbacks for the new interactive marker + interactive_marker_server_->setCallback( + model_name, + boost::bind(&InteractiveMarkerManager::processMouseUpFeedback, this, _1), + visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP); + interactive_marker_server_->setCallback( + model_name, + boost::bind(&InteractiveMarkerManager::processMouseDownFeedback, this, + _1), + visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN); + interactive_marker_server_->setCallback( + model_name, + boost::bind(&InteractiveMarkerManager::processPoseUpdateFeedback, this, + _1), + visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE); + + // Add context menu to the new interactive marker + menu_handler_.apply(*interactive_marker_server_, model_name); + + // Apply changes to server + interactive_marker_server_->applyChanges(); +} + +void InteractiveMarkerManager::deleteModelMenuCallback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + // Delete the model just as when the DeleteModel service is called + for (unsigned int i = 0; i < (*models_).size(); i++) { + if ((*models_)[i]->GetName() == feedback->marker_name) { + // delete the plugins associated with the model + plugin_manager_->DeleteModelPlugin((*models_)[i]); + delete (*models_)[i]; + (*models_).erase((*models_).begin() + i); + + // Also remove corresponding interactive marker + deleteInteractiveMarker(feedback->marker_name); + break; + } + } + + // Update menu handler and server + menu_handler_.apply(*interactive_marker_server_, feedback->marker_name); + interactive_marker_server_->applyChanges(); +} + +void InteractiveMarkerManager::deleteInteractiveMarker( + const std::string &model_name) { + // Remove target interactive marker by name and + // update the server + interactive_marker_server_->erase(model_name); + interactive_marker_server_->applyChanges(); +} + +void InteractiveMarkerManager::processMouseUpFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + // Update model that was manipulated the same way + // as when the MoveModel service is called + for (unsigned int i = 0; i < models_->size(); i++) { + if ((*models_)[i]->GetName() == feedback->marker_name) { + Pose new_pose; + new_pose.x = feedback->pose.position.x; + new_pose.y = feedback->pose.position.y; + new_pose.theta = atan2( + 2.0 * feedback->pose.orientation.w * feedback->pose.orientation.z, + 1.0 - + 2.0 * feedback->pose.orientation.z * + feedback->pose.orientation.z); + (*models_)[i]->SetPose(new_pose); + break; + } + } + manipulating_model_ = false; + interactive_marker_server_->applyChanges(); +} + +void InteractiveMarkerManager::processMouseDownFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + manipulating_model_ = true; +} + +void InteractiveMarkerManager::processPoseUpdateFeedback( + const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + pose_update_stamp_ = ros::WallTime::now(); +} + +void InteractiveMarkerManager::update() { + // Loop through each model, extract the pose of the root body, + // and use it to update the interactive marker pose. Only + // necessary to compute if user is not currently dragging + // an interactive marker + if (!manipulating_model_) { + for (size_t i = 0; i < (*models_).size(); i++) { + b2BodyId root_body = (*models_)[i]->bodies_[0]->physics_body_; + geometry_msgs::Pose new_pose; + new_pose.position.x = b2Body_GetPosition(root_body).x; + new_pose.position.y = b2Body_GetPosition(root_body).y; + double theta = b2Rot_GetAngle(b2Body_GetRotation(root_body)); + new_pose.orientation.w = cos(0.5 * theta); + new_pose.orientation.z = sin(0.5 * theta); + interactive_marker_server_->setPose((*models_)[i]->GetName(), new_pose); + interactive_marker_server_->applyChanges(); + } + } + + // Detect when interaction stops without triggering a MOUSE_UP event by + // monitoring the time since the last pose update feedback, which comes + // in at 33 Hz if the user is dragging the marker. When the stream of + // pose update feedback stops, automatically clear the manipulating_model_ + // flag to unpause the simulation. + double dt = 0; + try { + dt = (ros::WallTime::now() - pose_update_stamp_).toSec(); + } catch (std::runtime_error &ex) { + ROS_ERROR( + "Flatland Interactive Marker Manager runtime error: (%f - %f) [%s]", + ros::WallTime::now().toSec(), pose_update_stamp_.toSec(), ex.what()); + } + if (manipulating_model_ && dt > 0.1 && dt < 1.0) { + manipulating_model_ = false; + } +} + +InteractiveMarkerManager::~InteractiveMarkerManager() { + interactive_marker_server_.reset(); +} +} diff --git a/flatland_server/src/joint.cpp b/flatland_server/src/joint.cpp index 538c4236..63e78703 100644 --- a/flatland_server/src/joint.cpp +++ b/flatland_server/src/joint.cpp @@ -1,205 +1,209 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name joint.cpp - * @brief Implements Joint - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -namespace flatland_server { - -Joint::Joint(b2World *physics_world, Model *model, const std::string &name, - const Color &color, const b2JointDef &joint_def) - : model_(model), name_(name), physics_world_(physics_world), color_(color) { - physics_joint_ = physics_world->CreateJoint(&joint_def); - physics_joint_->SetUserData(this); -} - -Joint::~Joint() { physics_world_->DestroyJoint(physics_joint_); } - -Model *Joint::GetModel() { return model_; } - -const std::string &Joint::GetName() const { return name_; } - -const Color &Joint::GetColor() const { return color_; } - -void Joint::SetColor(const Color &color) { color_ = color; } - -b2Joint *Joint::GetPhysicsJoint() { return physics_joint_; } - -b2World *Joint::GetphysicsWorld() { return physics_world_; } - -Joint *Joint::MakeJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader) { - Joint *j; - - std::string name = joint_reader.Get("name"); - joint_reader.SetErrorInfo("model " + Q(model->name_), "joint " + Q(name)); - - std::string type = joint_reader.Get("type"); - Color color = joint_reader.GetColor("color", Color(1, 1, 1, 0.5)); - bool collide_connected = joint_reader.Get("collide_connected", false); - - YamlReader bodies_reader = joint_reader.Subnode("bodies", YamlReader::LIST); - if (bodies_reader.NodeSize() != 2) { - throw YAMLException("Invalid \"bodies\" in " + - bodies_reader.entry_location_ + - ", must be a sequence of exactly two items"); - } - - Vec2 anchors[2]; - ModelBody *bodies[2]; - for (unsigned int i = 0; i < 2; i++) { - YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); - std::string body_name = body_reader.Get("name"); - anchors[i] = body_reader.GetVec2("anchor"); - bodies[i] = model->GetBody(body_name); - - if (bodies[i] == nullptr) { - throw YAMLException("Cannot find body with name " + Q(body_name) + - " in " + body_reader.entry_location_ + " " + - body_reader.entry_name_); - } - } - - b2Vec2 anchor_A = anchors[0].Box2D(); - b2Vec2 anchor_B = anchors[1].Box2D(); - b2Body *body_A = bodies[0]->physics_body_; - b2Body *body_B = bodies[1]->physics_body_; - - if (type == "revolute") { - j = MakeRevoluteJoint(physics_world, model, joint_reader, name, color, - body_A, anchor_A, body_B, anchor_B, - collide_connected); - } else if (type == "weld") { - j = MakeWeldJoint(physics_world, model, joint_reader, name, color, body_A, - anchor_A, body_B, anchor_B, collide_connected); - } else { - throw YAMLException( - "Invalid joint \"type\" in " + joint_reader.entry_location_ + " " + - joint_reader.entry_name_ + ", supported joints are: revolute, weld"); - } - - try { - joint_reader.EnsureAccessedAllKeys(); - } catch (const YAMLException &e) { - delete j; - throw e; - } - return j; -} - -Joint *Joint::MakeRevoluteJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, - const std::string &name, const Color &color, - b2Body *body_A, b2Vec2 anchor_A, b2Body *body_B, - b2Vec2 anchor_B, bool collide_connected) { - double upper_limit, lower_limit; - bool has_limits = false; - - std::vector limits = joint_reader.GetList("limits", {}, 2, 2); - if (limits.size() == 2) { - lower_limit = limits[0]; - upper_limit = limits[1]; - has_limits = true; - } - - b2RevoluteJointDef joint_def; - joint_def.bodyA = body_A; - joint_def.bodyB = body_B; - joint_def.localAnchorA = anchor_A; - joint_def.localAnchorB = anchor_B; - joint_def.collideConnected = collide_connected; - - if (has_limits) { - joint_def.lowerAngle = lower_limit; - joint_def.upperAngle = upper_limit; - joint_def.enableLimit = true; - } else { - joint_def.enableLimit = false; - } - - return new Joint(physics_world, model, name, color, joint_def); -} - -Joint *Joint::MakeWeldJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, const std::string &name, - const Color &color, b2Body *body_A, b2Vec2 anchor_A, - b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected) { - double angle = joint_reader.Get("angle", 0.0); - double frequency = joint_reader.Get("frequency", 0.0); - double damping = joint_reader.Get("damping", 0.0); - - b2WeldJointDef joint_def; - joint_def.bodyA = body_A; - joint_def.bodyB = body_B; - joint_def.localAnchorA = anchor_A; - joint_def.localAnchorB = anchor_B; - joint_def.frequencyHz = frequency; - joint_def.dampingRatio = damping; - joint_def.referenceAngle = angle; - joint_def.collideConnected = collide_connected; - - return new Joint(physics_world, model, name, color, joint_def); -} - -void Joint::DebugOutput() const { - b2Joint *j = physics_joint_; - Body *body_A = static_cast(j->GetBodyA()->GetUserData()); - Body *body_B = static_cast(j->GetBodyB()->GetUserData()); - - ROS_DEBUG_NAMED("Joint", - "Joint %p: model(%p, %s) name(%s) color(%f,%f,%f,%f) " - "physics_joint(%p) body_A(%p, %s) anchor_A_world(%f, %f) " - "body_B(%p, %s) anchor_B_world(%f, %f)", - this, model_, model_->name_.c_str(), name_.c_str(), color_.r, - color_.g, color_.b, color_.a, physics_joint_, body_A, - body_A->name_.c_str(), j->GetAnchorA().x, j->GetAnchorA().y, - body_B, body_B->name_.c_str(), j->GetAnchorB().x, - j->GetAnchorB().y); -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name joint.cpp + * @brief Implements Joint + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace flatland_server { + +Joint::Joint(b2WorldId physics_world, Model *model, const std::string &name, + const Color &color, b2JointId joint_id) + : model_(model), name_(name), physics_world_(physics_world), color_(color) { + physics_joint_ = joint_id; +} + +Joint::~Joint() { + if (b2Joint_IsValid(physics_joint_)) { + b2DestroyJoint(physics_joint_); + } +} + +Model *Joint::GetModel() { return model_; } + +const std::string &Joint::GetName() const { return name_; } + +const Color &Joint::GetColor() const { return color_; } + +void Joint::SetColor(const Color &color) { color_ = color; } + +b2JointId Joint::GetPhysicsJoint() { return physics_joint_; } + +b2WorldId Joint::GetphysicsWorld() { return physics_world_; } + +Joint *Joint::MakeJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader) { + Joint *j; + + std::string name = joint_reader.Get("name"); + joint_reader.SetErrorInfo("model " + Q(model->name_), "joint " + Q(name)); + + std::string type = joint_reader.Get("type"); + Color color = joint_reader.GetColor("color", Color(1, 1, 1, 0.5)); + bool collide_connected = joint_reader.Get("collide_connected", false); + + YamlReader bodies_reader = joint_reader.Subnode("bodies", YamlReader::LIST); + if (bodies_reader.NodeSize() != 2) { + throw YAMLException("Invalid \"bodies\" in " + + bodies_reader.entry_location_ + + ", must be a sequence of exactly two items"); + } + + Vec2 anchors[2]; + ModelBody *bodies[2]; + for (unsigned int i = 0; i < 2; i++) { + YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); + std::string body_name = body_reader.Get("name"); + anchors[i] = body_reader.GetVec2("anchor"); + bodies[i] = model->GetBody(body_name); + + if (bodies[i] == nullptr) { + throw YAMLException("Cannot find body with name " + Q(body_name) + + " in " + body_reader.entry_location_ + " " + + body_reader.entry_name_); + } + } + + b2Vec2 anchor_A = anchors[0].Box2D(); + b2Vec2 anchor_B = anchors[1].Box2D(); + b2BodyId body_A = bodies[0]->physics_body_; + b2BodyId body_B = bodies[1]->physics_body_; + + if (type == "revolute") { + j = MakeRevoluteJoint(physics_world, model, joint_reader, name, color, + body_A, anchor_A, body_B, anchor_B, + collide_connected); + } else if (type == "weld") { + j = MakeWeldJoint(physics_world, model, joint_reader, name, color, body_A, + anchor_A, body_B, anchor_B, collide_connected); + } else { + throw YAMLException( + "Invalid joint \"type\" in " + joint_reader.entry_location_ + " " + + joint_reader.entry_name_ + ", supported joints are: revolute, weld"); + } + + try { + joint_reader.EnsureAccessedAllKeys(); + } catch (const YAMLException &e) { + delete j; + throw e; + } + return j; +} + +Joint *Joint::MakeRevoluteJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader, + const std::string &name, const Color &color, + b2BodyId body_A, b2Vec2 anchor_A, b2BodyId body_B, + b2Vec2 anchor_B, bool collide_connected) { + double upper_limit, lower_limit; + bool has_limits = false; + + std::vector limits = joint_reader.GetList("limits", {}, 2, 2); + if (limits.size() == 2) { + lower_limit = limits[0]; + upper_limit = limits[1]; + has_limits = true; + } + + b2RevoluteJointDef joint_def = b2DefaultRevoluteJointDef(); + joint_def.bodyIdA = body_A; + joint_def.bodyIdB = body_B; + joint_def.localAnchorA = anchor_A; + joint_def.localAnchorB = anchor_B; + joint_def.collideConnected = collide_connected; + + if (has_limits) { + joint_def.lowerAngle = static_cast(lower_limit); + joint_def.upperAngle = static_cast(upper_limit); + joint_def.enableLimit = true; + } else { + joint_def.enableLimit = false; + } + + b2JointId jid = b2CreateRevoluteJoint(physics_world, &joint_def); + return new Joint(physics_world, model, name, color, jid); +} + +Joint *Joint::MakeWeldJoint(b2WorldId physics_world, Model *model, + YamlReader &joint_reader, const std::string &name, + const Color &color, b2BodyId body_A, b2Vec2 anchor_A, + b2BodyId body_B, b2Vec2 anchor_B, + bool collide_connected) { + double angle = joint_reader.Get("angle", 0.0); + double frequency = joint_reader.Get("frequency", 0.0); + double damping = joint_reader.Get("damping", 0.0); + + b2WeldJointDef joint_def = b2DefaultWeldJointDef(); + joint_def.bodyIdA = body_A; + joint_def.bodyIdB = body_B; + joint_def.localAnchorA = anchor_A; + joint_def.localAnchorB = anchor_B; + joint_def.angularHertz = static_cast(frequency); + joint_def.angularDampingRatio = static_cast(damping); + joint_def.referenceAngle = static_cast(angle); + joint_def.collideConnected = collide_connected; + + b2JointId jid = b2CreateWeldJoint(physics_world, &joint_def); + return new Joint(physics_world, model, name, color, jid); +} + +void Joint::DebugOutput() const { + b2BodyId body_A_id = b2Joint_GetBodyA(physics_joint_); + b2BodyId body_B_id = b2Joint_GetBodyB(physics_joint_); + Body *body_A = static_cast(b2Body_GetUserData(body_A_id)); + Body *body_B = static_cast(b2Body_GetUserData(body_B_id)); + + ROS_DEBUG_NAMED("Joint", + "Joint %p: model(%p, %s) name(%s) color(%f,%f,%f,%f) " + "body_A(%p, %s) body_B(%p, %s)", + this, model_, model_->name_.c_str(), name_.c_str(), color_.r, + color_.g, color_.b, color_.a, + body_A, body_A ? body_A->name_.c_str() : "?", + body_B, body_B ? body_B->name_.c_str() : "?"); +} + +}; // namespace flatland_server diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp index e033dfc2..e3912398 100644 --- a/flatland_server/src/layer.cpp +++ b/flatland_server/src/layer.cpp @@ -1,347 +1,347 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world.cpp - * @brief implements flatland layer - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if CV_MAJOR_VERSION < 3 -#define GREYSCALE CV_LOAD_IMAGE_GRAYSCALE -#else -#include -#define GREYSCALE cv::ImreadModes::IMREAD_GRAYSCALE -#endif -#include - -namespace flatland_server { - -Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, - double resolution, const YAML::Node &properties) - : Entity(physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layer/" + names[0]) { - body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, - properties); - - LoadFromBitmap(bitmap, occupied_thresh, resolution); -} - -Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const std::vector &line_segments, - double scale, const YAML::Node &properties) - : Entity(physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layer/" + names[0]) { - body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, - properties); - - uint16_t category_bits = cfr_->GetCategoryBits(names_); - - for (const auto &line_segment : line_segments) { - b2EdgeShape edge; - edge.Set(line_segment.start.Box2D(), line_segment.end.Box2D()); - edge.m_vertex1 *= scale; - edge.m_vertex2 *= scale; - - b2FixtureDef fixture_def; - fixture_def.shape = &edge; - fixture_def.filter.categoryBits = category_bits; - fixture_def.filter.maskBits = fixture_def.filter.categoryBits; - // todo: add material information - body_->physics_body_->CreateFixture(&fixture_def); - } -} - -Layer::Layer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const YAML::Node &properties) - : Entity(physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layer/" + names[0]) {} - -Layer::~Layer() { delete body_; } - -const std::vector &Layer::GetNames() const { return names_; } - -const CollisionFilterRegistry *Layer::GetCfr() const { return cfr_; } -Body *Layer::GetBody() { return body_; } - -Layer *Layer::MakeLayer(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &map_path, - const std::vector &names, - const Color &color, const YAML::Node &properties) { - if (map_path.length() > 0) { // If there is a map in this layer - YamlReader reader(map_path); - reader.SetErrorInfo("layer " + Q(names[0])); - - std::string type = reader.Get("type", ""); - - if (type == "line_segments") { - double scale = reader.Get("scale"); - Pose origin = reader.GetPose("origin"); - boost::filesystem::path data_path(reader.Get("data")); - if (data_path.string().front() != '/') { - data_path = boost::filesystem::path(map_path).parent_path() / data_path; - } - - ROS_INFO_NAMED("Layer", - "layer \"%s\" loading line segments from path=\"%s\"", - names[0].c_str(), data_path.string().c_str()); - - std::vector line_segments; - - ReadLineSegmentsFile(data_path.string(), line_segments); - - return new Layer(physics_world, cfr, names, color, origin, line_segments, - scale, properties); - - } else { - double resolution = reader.Get("resolution"); - double occupied_thresh = reader.Get("occupied_thresh"); - Pose origin = reader.GetPose("origin"); - - boost::filesystem::path image_path(reader.Get("image")); - if (image_path.string().front() != '/') { - image_path = - boost::filesystem::path(map_path).parent_path() / image_path; - } - - ROS_INFO_NAMED("Layer", "layer \"%s\" loading image from path=\"%s\"", - names[0].c_str(), image_path.string().c_str()); - - cv::Mat map = cv::imread(image_path.string(), GREYSCALE); - if (map.empty()) { - throw YAMLException("Failed to load " + Q(image_path.string()) + - " in layer " + Q(names[0])); - } - - cv::Mat bitmap; - map.convertTo(bitmap, CV_32FC1, 1.0 / 255.0); - - return new Layer(physics_world, cfr, names, color, origin, bitmap, - occupied_thresh, resolution, properties); - } - } else { // If the layer has no static obstacles - return new Layer(physics_world, cfr, names, color, properties); - } -} - -void Layer::ReadLineSegmentsFile(const std::string &file_path, - std::vector &line_segments) { - std::ifstream in_file(file_path); - std::string line; - int line_count = 0; - - line_segments.clear(); - - if (in_file.fail()) { - throw Exception("Flatland File: Failed to load " + Q(file_path)); - } - - while (std::getline(in_file, line)) { - line_count++; - std::stringstream ss(line); - float n[4]; - - for (unsigned int i = 0; i < 4; i++) { - ss >> n[i]; - - if (ss.fail()) { - throw Exception( - "Flatland File: Failed to read line segment from line " + - std::to_string(line_count) + ", in file " + - Q(boost::filesystem::path(file_path).filename().string())); - } - } - - line_segments.push_back(LineSegment(Vec2(n[0], n[1]), Vec2(n[2], n[3]))); - } -} - -void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, - double resolution) { - uint16_t category_bits = cfr_->GetCategoryBits(names_); - - auto add_edge = [&](double x1, double y1, double x2, double y2) { - b2EdgeShape edge; - double rows = bitmap.rows; - double res = resolution; - - edge.Set(b2Vec2(res * x1, res * (rows - y1)), - b2Vec2(res * x2, res * (rows - y2))); - - b2FixtureDef fixture_def; - fixture_def.shape = &edge; - fixture_def.filter.categoryBits = category_bits; - fixture_def.filter.maskBits = fixture_def.filter.categoryBits; - body_->physics_body_->CreateFixture(&fixture_def); - }; - - cv::Mat padded_map, obstacle_map; - - // thresholds the map, values between the occupied threshold and 1.0 are - // considered to be occupied - cv::inRange(bitmap, occupied_thresh, 1.0, obstacle_map); - - // pad the top and bottom of the map each with an empty row (255=white). This - // helps to look at the transition from one row of pixel to another - cv::copyMakeBorder(obstacle_map, padded_map, 1, 1, 0, 0, cv::BORDER_CONSTANT, - 255); - - // loop through all the rows, looking at 2 at once - for (int i = 0; i < padded_map.rows - 1; i++) { - cv::Mat row1 = padded_map.row(i); - cv::Mat row2 = padded_map.row(i + 1); - cv::Mat diff; - - // if the two row are the same value, there is no edge - // if the two rows are not the same value, there is an edge - // result is still binary, either 255 or 0 - cv::absdiff(row1, row2, diff); - - int start = 0; - bool started = false; - - // find all the walls, put the connected walls as a single line segment - for (unsigned int j = 0; j <= diff.total(); j++) { - bool edge_exists = false; - if (j < diff.total()) { - edge_exists = diff.at(0, j); // 255 maps to true - } - - if (edge_exists && !started) { - start = j; - started = true; - } else if (started && !edge_exists) { - add_edge(start, i, j, i); - - started = false; - } - } - } - - // pad the left and right of the map each with an empty column (255). - cv::copyMakeBorder(obstacle_map, padded_map, 0, 0, 1, 1, cv::BORDER_CONSTANT, - 255); - - // loop through all the columns, looking at 2 at once - for (int i = 0; i < padded_map.cols - 1; i++) { - cv::Mat col1 = padded_map.col(i); - cv::Mat col2 = padded_map.col(i + 1); - cv::Mat diff; - - cv::absdiff(col1, col2, diff); - - int start = 0; - bool started = false; - - for (unsigned int j = 0; j <= diff.total(); j++) { - bool edge_exists = false; - if (j < diff.total()) { - edge_exists = diff.at(j, 0); - } - - if (edge_exists && !started) { - start = j; - started = true; - } else if (started && !edge_exists) { - add_edge(i, start, i, j); - - started = false; - } - } - } -} - -void Layer::DebugVisualize() const { - // Don't try to visualized uninitalized layers - if (viz_name_.length() == 0) { - return; - } - - DebugVisualization::Get().Reset(viz_name_); - DebugVisualization::Get().Reset(viz_name_ + "_3d"); - - if (body_ != nullptr) { - DebugVisualization::Get().Visualize(viz_name_, body_->physics_body_, - body_->color_.r, body_->color_.g, - body_->color_.b, body_->color_.a); - DebugVisualization::Get().VisualizeLayer(viz_name_ + "_3d", body_); - } -} - -void Layer::DebugOutput() const { - std::string names = "{" + boost::algorithm::join(names_, ",") + "}"; - uint16_t category_bits = cfr_->GetCategoryBits(names_); - - ROS_DEBUG_NAMED("Layer", - "Layer %p: physics_world(%p) name(%s) names(%s) " - "category_bits(0x%X)", - this, physics_world_, name_.c_str(), names.c_str(), - category_bits); - - if (body_ != nullptr) { - body_->DebugOutput(); - } -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world.cpp + * @brief implements flatland layer + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if CV_MAJOR_VERSION < 3 +#define GREYSCALE CV_LOAD_IMAGE_GRAYSCALE +#else +#include +#define GREYSCALE cv::ImreadModes::IMREAD_GRAYSCALE +#endif +#include + +namespace flatland_server { + +Layer::Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, + double resolution, const YAML::Node &properties) + : Entity(physics_world, names[0]), + names_(names), + cfr_(cfr), + viz_name_("layer/" + names[0]) { + body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, + properties); + + LoadFromBitmap(bitmap, occupied_thresh, resolution); +} + +Layer::Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const Pose &origin, const std::vector &line_segments, + double scale, const YAML::Node &properties) + : Entity(physics_world, names[0]), + names_(names), + cfr_(cfr), + viz_name_("layer/" + names[0]) { + body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, + properties); + + uint16_t category_bits = cfr_->GetCategoryBits(names_); + + for (const auto &line_segment : line_segments) { + b2Vec2 sv = line_segment.start.Box2D(); + b2Vec2 ev = line_segment.end.Box2D(); + sv.x *= static_cast(scale); sv.y *= static_cast(scale); + ev.x *= static_cast(scale); ev.y *= static_cast(scale); + + b2ShapeDef shape_def = b2DefaultShapeDef(); + shape_def.filter.categoryBits = category_bits; + shape_def.filter.maskBits = category_bits; + b2Segment seg = {sv, ev}; + // todo: add material information + b2CreateSegmentShape(body_->physics_body_, &shape_def, &seg); + } +} + +Layer::Layer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::vector &names, const Color &color, + const YAML::Node &properties) + : Entity(physics_world, names[0]), + names_(names), + cfr_(cfr), + viz_name_("layer/" + names[0]) {} + +Layer::~Layer() { delete body_; } + +const std::vector &Layer::GetNames() const { return names_; } + +const CollisionFilterRegistry *Layer::GetCfr() const { return cfr_; } +Body *Layer::GetBody() { return body_; } + +Layer *Layer::MakeLayer(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &map_path, + const std::vector &names, + const Color &color, const YAML::Node &properties) { + if (map_path.length() > 0) { // If there is a map in this layer + YamlReader reader(map_path); + reader.SetErrorInfo("layer " + Q(names[0])); + + std::string type = reader.Get("type", ""); + + if (type == "line_segments") { + double scale = reader.Get("scale"); + Pose origin = reader.GetPose("origin"); + boost::filesystem::path data_path(reader.Get("data")); + if (data_path.string().front() != '/') { + data_path = boost::filesystem::path(map_path).parent_path() / data_path; + } + + ROS_INFO_NAMED("Layer", + "layer \"%s\" loading line segments from path=\"%s\"", + names[0].c_str(), data_path.string().c_str()); + + std::vector line_segments; + + ReadLineSegmentsFile(data_path.string(), line_segments); + + return new Layer(physics_world, cfr, names, color, origin, line_segments, + scale, properties); + + } else { + double resolution = reader.Get("resolution"); + double occupied_thresh = reader.Get("occupied_thresh"); + Pose origin = reader.GetPose("origin"); + + boost::filesystem::path image_path(reader.Get("image")); + if (image_path.string().front() != '/') { + image_path = + boost::filesystem::path(map_path).parent_path() / image_path; + } + + ROS_INFO_NAMED("Layer", "layer \"%s\" loading image from path=\"%s\"", + names[0].c_str(), image_path.string().c_str()); + + cv::Mat map = cv::imread(image_path.string(), GREYSCALE); + if (map.empty()) { + throw YAMLException("Failed to load " + Q(image_path.string()) + + " in layer " + Q(names[0])); + } + + cv::Mat bitmap; + map.convertTo(bitmap, CV_32FC1, 1.0 / 255.0); + + return new Layer(physics_world, cfr, names, color, origin, bitmap, + occupied_thresh, resolution, properties); + } + } else { // If the layer has no static obstacles + return new Layer(physics_world, cfr, names, color, properties); + } +} + +void Layer::ReadLineSegmentsFile(const std::string &file_path, + std::vector &line_segments) { + std::ifstream in_file(file_path); + std::string line; + int line_count = 0; + + line_segments.clear(); + + if (in_file.fail()) { + throw Exception("Flatland File: Failed to load " + Q(file_path)); + } + + while (std::getline(in_file, line)) { + line_count++; + std::stringstream ss(line); + float n[4]; + + for (unsigned int i = 0; i < 4; i++) { + ss >> n[i]; + + if (ss.fail()) { + throw Exception( + "Flatland File: Failed to read line segment from line " + + std::to_string(line_count) + ", in file " + + Q(boost::filesystem::path(file_path).filename().string())); + } + } + + line_segments.push_back(LineSegment(Vec2(n[0], n[1]), Vec2(n[2], n[3]))); + } +} + +void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, + double resolution) { + uint16_t category_bits = cfr_->GetCategoryBits(names_); + + auto add_edge = [&](double x1, double y1, double x2, double y2) { + double rows = bitmap.rows; + double res = resolution; + + b2Vec2 v1 = {static_cast(res * x1), static_cast(res * (rows - y1))}; + b2Vec2 v2 = {static_cast(res * x2), static_cast(res * (rows - y2))}; + b2Segment seg = {v1, v2}; + + b2ShapeDef shape_def = b2DefaultShapeDef(); + shape_def.filter.categoryBits = category_bits; + shape_def.filter.maskBits = category_bits; + b2CreateSegmentShape(body_->physics_body_, &shape_def, &seg); + }; + + cv::Mat padded_map, obstacle_map; + + // thresholds the map, values between the occupied threshold and 1.0 are + // considered to be occupied + cv::inRange(bitmap, occupied_thresh, 1.0, obstacle_map); + + // pad the top and bottom of the map each with an empty row (255=white). This + // helps to look at the transition from one row of pixel to another + cv::copyMakeBorder(obstacle_map, padded_map, 1, 1, 0, 0, cv::BORDER_CONSTANT, + 255); + + // loop through all the rows, looking at 2 at once + for (int i = 0; i < padded_map.rows - 1; i++) { + cv::Mat row1 = padded_map.row(i); + cv::Mat row2 = padded_map.row(i + 1); + cv::Mat diff; + + // if the two row are the same value, there is no edge + // if the two rows are not the same value, there is an edge + // result is still binary, either 255 or 0 + cv::absdiff(row1, row2, diff); + + int start = 0; + bool started = false; + + // find all the walls, put the connected walls as a single line segment + for (unsigned int j = 0; j <= diff.total(); j++) { + bool edge_exists = false; + if (j < diff.total()) { + edge_exists = diff.at(0, j); // 255 maps to true + } + + if (edge_exists && !started) { + start = j; + started = true; + } else if (started && !edge_exists) { + add_edge(start, i, j, i); + + started = false; + } + } + } + + // pad the left and right of the map each with an empty column (255). + cv::copyMakeBorder(obstacle_map, padded_map, 0, 0, 1, 1, cv::BORDER_CONSTANT, + 255); + + // loop through all the columns, looking at 2 at once + for (int i = 0; i < padded_map.cols - 1; i++) { + cv::Mat col1 = padded_map.col(i); + cv::Mat col2 = padded_map.col(i + 1); + cv::Mat diff; + + cv::absdiff(col1, col2, diff); + + int start = 0; + bool started = false; + + for (unsigned int j = 0; j <= diff.total(); j++) { + bool edge_exists = false; + if (j < diff.total()) { + edge_exists = diff.at(j, 0); + } + + if (edge_exists && !started) { + start = j; + started = true; + } else if (started && !edge_exists) { + add_edge(i, start, i, j); + + started = false; + } + } + } +} + +void Layer::DebugVisualize() const { + // Don't try to visualized uninitalized layers + if (viz_name_.length() == 0) { + return; + } + + DebugVisualization::Get().Reset(viz_name_); + DebugVisualization::Get().Reset(viz_name_ + "_3d"); + + if (body_ != nullptr) { + DebugVisualization::Get().Visualize(viz_name_, body_->physics_body_, + body_->color_.r, body_->color_.g, + body_->color_.b, body_->color_.a); + DebugVisualization::Get().VisualizeLayer(viz_name_ + "_3d", body_); + } +} + +void Layer::DebugOutput() const { + std::string names = "{" + boost::algorithm::join(names_, ",") + "}"; + uint16_t category_bits = cfr_->GetCategoryBits(names_); + + ROS_DEBUG_NAMED("Layer", + "Layer %p: physics_world(id=%d,%d) name(%s) names(%s) " + "category_bits(0x%X)", + this, physics_world_.index1, physics_world_.generation, + name_.c_str(), names.c_str(), + category_bits); + + if (body_ != nullptr) { + body_->DebugOutput(); + } +} + +}; // namespace flatland_server diff --git a/flatland_server/src/message_server.cpp b/flatland_server/src/message_server.cpp new file mode 100644 index 00000000..ab9039ce --- /dev/null +++ b/flatland_server/src/message_server.cpp @@ -0,0 +1,3 @@ +#include + +namespace flatland_server {} diff --git a/flatland_server/src/model.cpp b/flatland_server/src/model.cpp index 5670d7d1..4d44b84b 100644 --- a/flatland_server/src/model.cpp +++ b/flatland_server/src/model.cpp @@ -1,305 +1,350 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model.cpp - * @brief implements flatland model - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -namespace flatland_server { - -Model::Model(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &ns, const std::string &name) - : Entity(physics_world, name), - namespace_(ns), - cfr_(cfr), - viz_name_("model/" + name_) {} - -Model::~Model() { - for (unsigned int i = 0; i < joints_.size(); i++) { - delete joints_[i]; - } - - for (unsigned int i = 0; i < bodies_.size(); i++) { - delete bodies_[i]; - } - - // No need to destroy joints since destruction of model will destroy the - // joint, the creation of a joint must always have bodies attached to it - - // clear visualization - DebugVisualization::Get().Reset(viz_name_); -} - -Model *Model::MakeModel(b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &model_yaml_path, - const std::string &ns, const std::string &name) { - YamlReader reader(model_yaml_path); - reader.SetErrorInfo("model " + Q(name)); - - Model *m = new Model(physics_world, cfr, ns, name); - - m->plugins_reader_ = reader.SubnodeOpt("plugins", YamlReader::LIST); - - try { - YamlReader bodies_reader = reader.Subnode("bodies", YamlReader::LIST); - YamlReader joints_reader = reader.SubnodeOpt("joints", YamlReader::LIST); - reader.EnsureAccessedAllKeys(); - - m->LoadBodies(bodies_reader); - m->LoadJoints(joints_reader); - } catch (const YAMLException &e) { - delete m; - throw e; - } - - return m; -} - -void Model::LoadBodies(YamlReader &bodies_reader) { - if (bodies_reader.NodeSize() <= 0) { - throw YAMLException("Invalid \"bodies\" in " + Q(name_) + - " model, must a be list of bodies of at least size 1"); - } else { - for (int i = 0; i < bodies_reader.NodeSize(); i++) { - YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); - if (!body_reader.Get("enabled", "true")) { - ROS_INFO_STREAM("Body " - << Q(name_) << "." - << body_reader.Get("name", "unnamed") - << " disabled"); - continue; - } - ModelBody *b = - ModelBody::MakeBody(physics_world_, cfr_, this, body_reader); - bodies_.push_back(b); - - // ensure body is not a duplicate - if (std::count_if(bodies_.begin(), bodies_.end(), - [&](Body *i) { return i->name_ == b->name_; }) > 1) { - throw YAMLException("Invalid \"bodies\" in " + Q(name_) + - " model, body with name " + Q(b->name_) + - " already exists"); - } - } - } -} - -void Model::LoadJoints(YamlReader &joints_reader) { - if (!joints_reader.IsNodeNull()) { - for (int i = 0; i < joints_reader.NodeSize(); i++) { - YamlReader joint_reader = joints_reader.Subnode(i, YamlReader::MAP); - if (!joint_reader.Get("enabled", "true")) { - ROS_INFO_STREAM("Joint " - << Q(name_) << "." - << joint_reader.Get("name", "unnamed") - << " disabled"); - continue; - } - Joint *j = Joint::MakeJoint(physics_world_, this, joint_reader); - joints_.push_back(j); - - // ensure joint is not a duplicate - if (std::count_if(joints_.begin(), joints_.end(), - [&](Joint *i) { return i->name_ == j->name_; }) > 1) { - throw YAMLException("Invalid \"joints\" in " + Q(name_) + - " model, joint with name " + Q(j->name_) + - " already exists"); - } - } - } -} - -ModelBody *Model::GetBody(const std::string &name) { - for (unsigned int i = 0; i < bodies_.size(); i++) { - if (bodies_[i]->name_ == name) { - return bodies_[i]; - } - } - return nullptr; -} - -Joint *Model::GetJoint(const std::string &name) { - for (unsigned int i = 0; i < joints_.size(); i++) { - if (joints_[i]->name_ == name) { - return joints_[i]; - } - } - return nullptr; -} - -const std::vector &Model::GetBodies() { return bodies_; } - -const std::vector &Model::GetJoints() { return joints_; } - -const std::string &Model::GetNameSpace() const { return namespace_; } - -std::string Model::NameSpaceTF(const std::string &frame_id) const { - // case: "global" namespace: don't prefix, strip leading slash - if (frame_id.substr(0, 1) == "/") { - return std::string(frame_id, 1, - std::string::npos); // Strip the leading '/' - } else { // case: "local" namespace: prepend namespace - if (namespace_.length() > 0) { - return namespace_ + "_" + frame_id; - } else { - return frame_id; - } - } -} - -std::string Model::NameSpaceTopic(const std::string &topic) const { - // We don't actually want the topic to be "global" in case flatland is itself - // namespaced, so strip leading slash - if (topic.substr(0, 1) == "/") { - return std::string(topic, 1, std::string::npos); // Strip the leading '/' - } else { // case: "local" namespace: prepend namespace/ - if (namespace_.length() > 0) { - return namespace_ + "/" + topic; - } else { - return topic; - } - } -} - -const std::string &Model::GetName() const { return name_; } - -const CollisionFilterRegistry *Model::GetCfr() const { return cfr_; } - -void Model::SetPose(const Pose &pose) { - // Grab first (root?) body transform - RotateTranslate root_body_transform = - Geometry::CreateTransform(bodies_[0]->physics_body_->GetPosition().x, - bodies_[0]->physics_body_->GetPosition().y, - bodies_[0]->physics_body_->GetAngle()); - - // Inverse transform all bodies by this to reset their poses - for (unsigned int i = 0; i < bodies_.size(); i++) { - bodies_[i]->physics_body_->SetTransform( - Geometry::InverseTransform(bodies_[i]->physics_body_->GetPosition(), - root_body_transform), - 0.0); - } - - // Apply new desired pose in world coordinates - TransformAll(pose); -} - -void Model::TransformAll(const Pose &pose_delta) { - // -- -- -- -- - // | cos(a) -sin(a) x | | cos(b) -sin(b) u | - // | sin(a) cos(a) y | x | sin(b) cos(b) v | - // | 0 0 1 | | 0 0 1 | - // -- -- -- -- - // -- -- - // | cos(a+b) -sin(a+b) x + u*cos(a) - v*sin(a) | - // = | sin(a+b) cos(a+b) y + u*sin(a) + v*cos(a) | - // | 0 0 1 | - // -- -- - - RotateTranslate tf = - Geometry::CreateTransform(pose_delta.x, pose_delta.y, pose_delta.theta); - - for (unsigned int i = 0; i < bodies_.size(); i++) { - bodies_[i]->physics_body_->SetTransform( - Geometry::Transform(bodies_[i]->physics_body_->GetPosition(), tf), - bodies_[i]->physics_body_->GetAngle() + pose_delta.theta); - } -} - -void Model::DebugVisualize() const { - DebugVisualization::Get().Reset(viz_name_); - - for (const auto &body : bodies_) { - DebugVisualization::Get().Visualize(viz_name_, body->physics_body_, - body->color_.r, body->color_.g, - body->color_.b, body->color_.a); - } - - for (const auto &joint : joints_) { - DebugVisualization::Get().Visualize(viz_name_, joint->physics_joint_, - joint->color_.r, joint->color_.g, - joint->color_.b, joint->color_.a); - } -} - -void Model::DebugOutput() const { - ROS_DEBUG_NAMED("Model", - "Model %p: physics_world(%p) name(%s) namespace(%s) " - "num_bodies(%lu) num_joints(%lu)", - this, physics_world_, name_.c_str(), namespace_.c_str(), - bodies_.size(), joints_.size()); - - for (const auto &body : bodies_) { - body->DebugOutput(); - } - - for (const auto &joint : joints_) { - joint->DebugOutput(); - } -} - -void Model::DumpBox2D() const { - for (const auto &body : bodies_) { - b2Log("BODY %p name=%s box2d_body=%p model=%p model_name=%s\n", body, - body->name_.c_str(), body->physics_body_, this, name_.c_str()); - body->physics_body_->Dump(); - } - - for (const auto &joint : joints_) { - Body *body_A = - static_cast(joint->physics_joint_->GetBodyA()->GetUserData()); - Body *body_B = - static_cast(joint->physics_joint_->GetBodyB()->GetUserData()); - b2Log( - "JOINT %p name=%s box2d_joint=%p model=%p model_name=%s " - "body_A(%p %s) body_B(%p %s)\n", - joint, joint->name_.c_str(), joint->physics_joint_, this, name_.c_str(), - body_A, body_A->name_.c_str(), body_B, body_B->name_.c_str()); - joint->physics_joint_->Dump(); - } -} -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model.cpp + * @brief implements flatland model + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +namespace flatland_server { + +Model::Model(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &ns, const std::string &name) + : Entity(physics_world, name), + world_(nullptr), + namespace_(ns), + cfr_(cfr), + viz_name_("model/" + name_) {} + +Model::Model(World *world, b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &ns, const std::string &name) + : Entity(physics_world, name), + world_(world), + namespace_(ns), + cfr_(cfr), + viz_name_("model/" + name_) {} + +World &Model::GetWorld() const { return *world_; } + +MessageServer &Model::GetMessageServer() const { + return GetWorld().message_server; +} + +Model::~Model() { + for (unsigned int i = 0; i < joints_.size(); i++) { + delete joints_[i]; + } + + for (unsigned int i = 0; i < bodies_.size(); i++) { + delete bodies_[i]; + } + + // No need to destroy joints since destruction of model will destroy the + // joint, the creation of a joint must always have bodies attached to it + + // clear visualization + DebugVisualization::Get().Reset(viz_name_); +} + +Model *Model::MakeModel(World *world, b2WorldId physics_world, + CollisionFilterRegistry *cfr, + const std::string &model_yaml_path, + const std::string &ns, const std::string &name) { + YamlReader reader(model_yaml_path); + reader.SetErrorInfo("model " + Q(name)); + + Model *m = new Model(world, physics_world, cfr, ns, name); + + m->plugins_reader_ = reader.SubnodeOpt("plugins", YamlReader::LIST); + + try { + YamlReader bodies_reader = reader.Subnode("bodies", YamlReader::LIST); + YamlReader joints_reader = reader.SubnodeOpt("joints", YamlReader::LIST); + reader.EnsureAccessedAllKeys(); + + m->LoadBodies(bodies_reader); + m->LoadJoints(joints_reader); + } catch (const YAMLException &e) { + delete m; + throw e; + } + + return m; +} + +Model *Model::MakeModel(b2WorldId physics_world, CollisionFilterRegistry *cfr, + const std::string &model_yaml_path, + const std::string &ns, const std::string &name) { + YamlReader reader(model_yaml_path); + reader.SetErrorInfo("model " + Q(name)); + + Model *m = new Model(physics_world, cfr, ns, name); + + m->plugins_reader_ = reader.SubnodeOpt("plugins", YamlReader::LIST); + + try { + YamlReader bodies_reader = reader.Subnode("bodies", YamlReader::LIST); + YamlReader joints_reader = reader.SubnodeOpt("joints", YamlReader::LIST); + reader.EnsureAccessedAllKeys(); + + m->LoadBodies(bodies_reader); + m->LoadJoints(joints_reader); + } catch (const YAMLException &e) { + delete m; + throw e; + } + + return m; +} + +void Model::LoadBodies(YamlReader &bodies_reader) { + if (bodies_reader.NodeSize() <= 0) { + throw YAMLException("Invalid \"bodies\" in " + Q(name_) + + " model, must a be list of bodies of at least size 1"); + } else { + for (int i = 0; i < bodies_reader.NodeSize(); i++) { + YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); + if (!body_reader.Get("enabled", "true")) { + ROS_INFO_STREAM("Body " + << Q(name_) << "." + << body_reader.Get("name", "unnamed") + << " disabled"); + continue; + } + ModelBody *b = + ModelBody::MakeBody(physics_world_, cfr_, this, body_reader); + bodies_.push_back(b); + + // ensure body is not a duplicate + if (std::count_if(bodies_.begin(), bodies_.end(), + [&](Body *i) { return i->name_ == b->name_; }) > 1) { + throw YAMLException("Invalid \"bodies\" in " + Q(name_) + + " model, body with name " + Q(b->name_) + + " already exists"); + } + } + } +} + +void Model::LoadJoints(YamlReader &joints_reader) { + if (!joints_reader.IsNodeNull()) { + for (int i = 0; i < joints_reader.NodeSize(); i++) { + YamlReader joint_reader = joints_reader.Subnode(i, YamlReader::MAP); + if (!joint_reader.Get("enabled", "true")) { + ROS_INFO_STREAM("Joint " + << Q(name_) << "." + << joint_reader.Get("name", "unnamed") + << " disabled"); + continue; + } + Joint *j = Joint::MakeJoint(physics_world_, this, joint_reader); + joints_.push_back(j); + + // ensure joint is not a duplicate + if (std::count_if(joints_.begin(), joints_.end(), + [&](Joint *i) { return i->name_ == j->name_; }) > 1) { + throw YAMLException("Invalid \"joints\" in " + Q(name_) + + " model, joint with name " + Q(j->name_) + + " already exists"); + } + } + } +} + +ModelBody *Model::GetBody(const std::string &name) { + for (unsigned int i = 0; i < bodies_.size(); i++) { + if (bodies_[i]->name_ == name) { + return bodies_[i]; + } + } + return nullptr; +} + +Joint *Model::GetJoint(const std::string &name) { + for (unsigned int i = 0; i < joints_.size(); i++) { + if (joints_[i]->name_ == name) { + return joints_[i]; + } + } + return nullptr; +} + +const std::vector &Model::GetBodies() { return bodies_; } + +const std::vector &Model::GetJoints() { return joints_; } + +const std::string &Model::GetNameSpace() const { return namespace_; } + +std::string Model::NameSpaceTF(const std::string &frame_id) const { + // case: "global" namespace: don't prefix, strip leading slash + if (frame_id.substr(0, 1) == "/") { + return std::string(frame_id, 1, + std::string::npos); // Strip the leading '/' + } else { // case: "local" namespace: prepend namespace + if (namespace_.length() > 0) { + return namespace_ + "_" + frame_id; + } else { + return frame_id; + } + } +} + +std::string Model::NameSpaceTopic(const std::string &topic) const { + // We don't actually want the topic to be "global" in case flatland is itself + // namespaced, so strip leading slash + if (topic.substr(0, 1) == "/") { + return std::string(topic, 1, std::string::npos); // Strip the leading '/' + } else { // case: "local" namespace: prepend namespace/ + if (namespace_.length() > 0) { + return namespace_ + "/" + topic; + } else { + return topic; + } + } +} + +const std::string &Model::GetName() const { return name_; } + +const CollisionFilterRegistry *Model::GetCfr() const { return cfr_; } + +void Model::SetPose(const Pose &pose) { + // Grab first (root?) body transform + b2BodyId root = bodies_[0]->physics_body_; + RotateTranslate root_body_transform = + Geometry::CreateTransform(b2Body_GetPosition(root).x, + b2Body_GetPosition(root).y, + b2Rot_GetAngle(b2Body_GetRotation(root))); + + // Inverse transform all bodies by this to reset their poses + for (unsigned int i = 0; i < bodies_.size(); i++) { + b2BodyId bid = bodies_[i]->physics_body_; + b2Vec2 new_pos = Geometry::InverseTransform(b2Body_GetPosition(bid), + root_body_transform); + b2Body_SetTransform(bid, new_pos, b2MakeRot(0.0f)); + } + + // Apply new desired pose in world coordinates + TransformAll(pose); +} + +void Model::TransformAll(const Pose &pose_delta) { + // -- -- -- -- + // | cos(a) -sin(a) x | | cos(b) -sin(b) u | + // | sin(a) cos(a) y | x | sin(b) cos(b) v | + // | 0 0 1 | | 0 0 1 | + // -- -- -- -- + // -- -- + // | cos(a+b) -sin(a+b) x + u*cos(a) - v*sin(a) | + // = | sin(a+b) cos(a+b) y + u*sin(a) + v*cos(a) | + // | 0 0 1 | + // -- -- + + RotateTranslate tf = + Geometry::CreateTransform(pose_delta.x, pose_delta.y, pose_delta.theta); + + for (unsigned int i = 0; i < bodies_.size(); i++) { + b2BodyId bid = bodies_[i]->physics_body_; + b2Vec2 new_pos = Geometry::Transform(b2Body_GetPosition(bid), tf); + float new_angle = b2Rot_GetAngle(b2Body_GetRotation(bid)) + + static_cast(pose_delta.theta); + b2Body_SetTransform(bid, new_pos, b2MakeRot(new_angle)); + } +} + +void Model::DebugVisualize() const { + DebugVisualization::Get().Reset(viz_name_); + + for (const auto &body : bodies_) { + DebugVisualization::Get().Visualize(viz_name_, body->physics_body_, + body->color_.r, body->color_.g, + body->color_.b, body->color_.a); + } + + for (const auto &joint : joints_) { + DebugVisualization::Get().Visualize(viz_name_, joint->physics_joint_, + joint->color_.r, joint->color_.g, + joint->color_.b, joint->color_.a); + } +} + +void Model::DebugOutput() const { + ROS_DEBUG_NAMED("Model", + "Model %p: physics_world(id=%d,%d) name(%s) namespace(%s) " + "num_bodies(%lu) num_joints(%lu)", + this, physics_world_.index1, physics_world_.generation, + name_.c_str(), namespace_.c_str(), + bodies_.size(), joints_.size()); + + for (const auto &body : bodies_) { + body->DebugOutput(); + } + + for (const auto &joint : joints_) { + joint->DebugOutput(); + } +} + +void Model::DumpBox2D() const { + for (const auto &body : bodies_) { + ROS_DEBUG_NAMED("Model", "BODY %p name=%s model=%p model_name=%s", body, + body->name_.c_str(), this, name_.c_str()); + } + + for (const auto &joint : joints_) { + b2BodyId bodyA_id = b2Joint_GetBodyA(joint->physics_joint_); + b2BodyId bodyB_id = b2Joint_GetBodyB(joint->physics_joint_); + Body *body_A = static_cast(b2Body_GetUserData(bodyA_id)); + Body *body_B = static_cast(b2Body_GetUserData(bodyB_id)); + ROS_DEBUG_NAMED("Model", + "JOINT %p name=%s model=%p model_name=%s " + "body_A(%p %s) body_B(%p %s)", + joint, joint->name_.c_str(), this, name_.c_str(), + body_A, body_A ? body_A->name_.c_str() : "(null)", + body_B, body_B ? body_B->name_.c_str() : "(null)"); + } +} +}; // namespace flatland_server diff --git a/flatland_server/src/model_body.cpp b/flatland_server/src/model_body.cpp index 1845dbab..f5f2105f 100644 --- a/flatland_server/src/model_body.cpp +++ b/flatland_server/src/model_body.cpp @@ -1,190 +1,186 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_body.cpp - * @brief implements flatland model_body - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -namespace flatland_server { - -ModelBody::ModelBody(b2World *physics_world, CollisionFilterRegistry *cfr, - Model *model, const std::string &name, const Color &color, - const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping, - double angular_damping) - : Body(physics_world, model, name, color, pose, body_type, properties, - linear_damping, angular_damping), - cfr_(cfr) {} - -const CollisionFilterRegistry *ModelBody::GetCfr() const { return cfr_; } - -ModelBody *ModelBody::MakeBody(b2World *physics_world, - CollisionFilterRegistry *cfr, Model *model, - YamlReader &body_reader) { - std::string name = body_reader.Get("name"); - body_reader.SetErrorInfo("model " + Q(model->name_), "body " + Q(name)); - - Pose pose = body_reader.GetPose("pose", Pose(0, 0, 0)); - Color color = body_reader.GetColor("color", Color(1, 1, 1, 0.5)); - std::string type_str = body_reader.Get("type", "dynamic"); - double linear_damping = body_reader.Get("linear_damping", 0.0); - double angular_damping = body_reader.Get("angular_damping", 0.0); - - b2BodyType type; - if (type_str == "static") { - type = b2_staticBody; - } else if (type_str == "kinematic") { - type = b2_kinematicBody; - } else if (type_str == "dynamic") { - type = b2_dynamicBody; - } else { - throw YAMLException("Invalid \"type\" in " + body_reader.entry_location_ + - " " + body_reader.entry_name_ + - ", must be one of: static, kinematic, dynamic"); - } - - // TODO: Read the model's properties - ModelBody *m = - new ModelBody(physics_world, cfr, model, name, color, pose, type, - YAML::Node(), linear_damping, angular_damping); - - try { - YamlReader footprints_node = - body_reader.Subnode("footprints", YamlReader::LIST); - body_reader.EnsureAccessedAllKeys(); - - m->LoadFootprints(footprints_node); - } catch (const YAMLException &e) { - delete m; - throw e; - } - - return m; -} - -void ModelBody::LoadFootprints(YamlReader &footprints_reader) { - for (int i = 0; i < footprints_reader.NodeSize(); i++) { - YamlReader reader = footprints_reader.Subnode(i, YamlReader::MAP); - - std::string type = reader.Get("type"); - if (type == "circle") { - LoadCircleFootprint(reader); - } else if (type == "polygon") { - LoadPolygonFootprint(reader); - } else { - throw YAMLException("Invalid footprint \"type\" in " + - reader.entry_location_ + " " + reader.entry_name_ + - ", support footprints are: circle, polygon"); - } - - reader.EnsureAccessedAllKeys(); - } -} - -void ModelBody::ConfigFootprintDef(YamlReader &footprint_reader, - b2FixtureDef &fixture_def) { - // configure physics properties - fixture_def.density = footprint_reader.Get("density"); - fixture_def.friction = footprint_reader.Get("friction", 0.0); - fixture_def.restitution = footprint_reader.Get("restitution", 0.0); - - // config collision properties - fixture_def.isSensor = footprint_reader.Get("sensor", false); - fixture_def.filter.groupIndex = 0; - - std::vector layers = - footprint_reader.GetList("layers", {"all"}, -1, -1); - std::vector invalid_layers; - fixture_def.filter.categoryBits = - cfr_->GetCategoryBits(layers, &invalid_layers); - - if (!invalid_layers.empty()) { - throw YAMLException("Invalid footprint \"layers\" in " + - footprint_reader.entry_location_ + " " + - footprint_reader.entry_name_ + ", {" + - boost::algorithm::join(invalid_layers, ",") + - "} layer(s) does not exist"); - } - - bool collision = footprint_reader.Get("collision", true); - if (collision) { - // b2d docs: maskBits are "I collide with" bitmask - fixture_def.filter.maskBits = fixture_def.filter.categoryBits; - } else { - // "I will collide with nothing" - fixture_def.filter.maskBits = 0; - } -} - -void ModelBody::LoadCircleFootprint(YamlReader &footprint_reader) { - Vec2 center = footprint_reader.GetVec2("center", Vec2(0, 0)); - double radius = footprint_reader.Get("radius"); - - b2FixtureDef fixture_def; - ConfigFootprintDef(footprint_reader, fixture_def); - - b2CircleShape shape; - shape.m_p.Set(center.x, center.y); - shape.m_radius = radius; - - fixture_def.shape = &shape; - physics_body_->CreateFixture(&fixture_def); -} - -void ModelBody::LoadPolygonFootprint(YamlReader &footprint_reader) { - std::vector points = - footprint_reader.GetList("points", 3, b2_maxPolygonVertices); - - b2FixtureDef fixture_def; - ConfigFootprintDef(footprint_reader, fixture_def); - - b2PolygonShape shape; - shape.Set(points.data(), points.size()); - - fixture_def.shape = &shape; - physics_body_->CreateFixture(&fixture_def); -} -}; +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_body.cpp + * @brief implements flatland model_body + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace flatland_server { + +ModelBody::ModelBody(b2WorldId physics_world, CollisionFilterRegistry *cfr, + Model *model, const std::string &name, const Color &color, + const Pose &pose, b2BodyType body_type, + const YAML::Node &properties, double linear_damping, + double angular_damping) + : Body(physics_world, model, name, color, pose, body_type, properties, + linear_damping, angular_damping), + cfr_(cfr) {} + +const CollisionFilterRegistry *ModelBody::GetCfr() const { return cfr_; } + +ModelBody *ModelBody::MakeBody(b2WorldId physics_world, + CollisionFilterRegistry *cfr, Model *model, + YamlReader &body_reader) { + std::string name = body_reader.Get("name"); + body_reader.SetErrorInfo("model " + Q(model->name_), "body " + Q(name)); + + Pose pose = body_reader.GetPose("pose", Pose(0, 0, 0)); + Color color = body_reader.GetColor("color", Color(1, 1, 1, 0.5)); + std::string type_str = body_reader.Get("type", "dynamic"); + double linear_damping = body_reader.Get("linear_damping", 0.0); + double angular_damping = body_reader.Get("angular_damping", 0.0); + + b2BodyType type; + if (type_str == "static") { + type = b2_staticBody; + } else if (type_str == "kinematic") { + type = b2_kinematicBody; + } else if (type_str == "dynamic") { + type = b2_dynamicBody; + } else { + throw YAMLException("Invalid \"type\" in " + body_reader.entry_location_ + + " " + body_reader.entry_name_ + + ", must be one of: static, kinematic, dynamic"); + } + + // TODO: Read the model's properties + ModelBody *m = + new ModelBody(physics_world, cfr, model, name, color, pose, type, + YAML::Node(), linear_damping, angular_damping); + + try { + YamlReader footprints_node = + body_reader.Subnode("footprints", YamlReader::LIST); + body_reader.EnsureAccessedAllKeys(); + + m->LoadFootprints(footprints_node); + } catch (const YAMLException &e) { + delete m; + throw e; + } + + return m; +} + +void ModelBody::LoadFootprints(YamlReader &footprints_reader) { + for (int i = 0; i < footprints_reader.NodeSize(); i++) { + YamlReader reader = footprints_reader.Subnode(i, YamlReader::MAP); + + std::string type = reader.Get("type"); + if (type == "circle") { + LoadCircleFootprint(reader); + } else if (type == "polygon") { + LoadPolygonFootprint(reader); + } else { + throw YAMLException("Invalid footprint \"type\" in " + + reader.entry_location_ + " " + reader.entry_name_ + + ", support footprints are: circle, polygon"); + } + + reader.EnsureAccessedAllKeys(); + } +} + +void ModelBody::ConfigFootprintDef(YamlReader &footprint_reader, + b2ShapeDef &shape_def) { + // configure physics properties + shape_def.density = footprint_reader.Get("density"); + shape_def.material.friction = footprint_reader.Get("friction", 0.0); + shape_def.material.restitution = footprint_reader.Get("restitution", 0.0); + + // config collision properties + shape_def.isSensor = footprint_reader.Get("sensor", false); + shape_def.filter.groupIndex = 0; + + std::vector layers = + footprint_reader.GetList("layers", {"all"}, -1, -1); + std::vector invalid_layers; + shape_def.filter.categoryBits = + cfr_->GetCategoryBits(layers, &invalid_layers); + + if (!invalid_layers.empty()) { + throw YAMLException("Invalid footprint \"layers\" in " + + footprint_reader.entry_location_ + " " + + footprint_reader.entry_name_ + ", {" + + boost::algorithm::join(invalid_layers, ",") + + "} layer(s) does not exist"); + } + + bool collision = footprint_reader.Get("collision", true); + if (collision) { + shape_def.filter.maskBits = shape_def.filter.categoryBits; + } else { + shape_def.filter.maskBits = 0; + } +} + +void ModelBody::LoadCircleFootprint(YamlReader &footprint_reader) { + Vec2 center = footprint_reader.GetVec2("center", Vec2(0, 0)); + double radius = footprint_reader.Get("radius"); + + b2ShapeDef shape_def = b2DefaultShapeDef(); + ConfigFootprintDef(footprint_reader, shape_def); + + b2Circle circle; + circle.center = {static_cast(center.x), static_cast(center.y)}; + circle.radius = static_cast(radius); + + b2CreateCircleShape(physics_body_, &shape_def, &circle); +} + +void ModelBody::LoadPolygonFootprint(YamlReader &footprint_reader) { + std::vector points = + footprint_reader.GetList("points", 3, B2_MAX_POLYGON_VERTICES); + + b2ShapeDef shape_def = b2DefaultShapeDef(); + ConfigFootprintDef(footprint_reader, shape_def); + + b2Hull hull = b2ComputeHull(points.data(), static_cast(points.size())); + b2Polygon polygon = b2MakePolygon(&hull, 0.0f); + + b2CreatePolygonShape(physics_body_, &shape_def, &polygon); +} +}; diff --git a/flatland_server/src/model_plugin.cpp b/flatland_server/src/model_plugin.cpp index 9d9774eb..1c47bbdc 100644 --- a/flatland_server/src/model_plugin.cpp +++ b/flatland_server/src/model_plugin.cpp @@ -1,93 +1,93 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_plugin.cpp - * @brief Implementation for ModelPlugin pluginlib plugins - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -namespace flatland_server { - -Model *ModelPlugin::GetModel() { return model_; } - -void ModelPlugin::Initialize(const std::string &type, const std::string &name, - Model *model, const YAML::Node &config) { - type_ = type; - name_ = name; - model_ = model; - plugin_type_ = PluginType::Model; - nh_ = ros::NodeHandle(model_->namespace_); - OnInitialize(config); -} - -bool ModelPlugin::FilterContact(b2Contact *contact, Entity *&entity, - b2Fixture *&this_fixture, - b2Fixture *&other_fixture) { - b2Fixture *f_A = contact->GetFixtureA(); - b2Fixture *f_B = contact->GetFixtureB(); - Body *b_A = static_cast(f_A->GetBody()->GetUserData()); - Body *b_B = static_cast(f_B->GetBody()->GetUserData()); - Entity *e_A = b_A->GetEntity(); - Entity *e_B = b_B->GetEntity(); - - if (e_A == model_) { - entity = e_B; - this_fixture = f_A; - other_fixture = f_B; - } else if (e_B == model_) { - entity = e_A; - this_fixture = f_B; - other_fixture = f_A; - } else { - return false; - } - return true; -} - -bool ModelPlugin::FilterContact(b2Contact *contact) { - b2Fixture *f1, *f2; - Entity *e; - return FilterContact(contact, e, f1, f2); -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_plugin.cpp + * @brief Implementation for ModelPlugin pluginlib plugins + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +namespace flatland_server { + +Model *ModelPlugin::GetModel() { return model_; } + +void ModelPlugin::Initialize(const std::string &type, const std::string &name, + Model *model, const YAML::Node &config) { + type_ = type; + name_ = name; + model_ = model; + plugin_type_ = PluginType::Model; + nh_ = ros::NodeHandle(model_->namespace_); + OnInitialize(config); +} + +bool ModelPlugin::FilterContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + Entity *&entity, b2BodyId &this_body, + b2BodyId &other_body) { + b2BodyId body_A = b2Shape_GetBody(shapeIdA); + b2BodyId body_B = b2Shape_GetBody(shapeIdB); + Body *b_A = static_cast(b2Body_GetUserData(body_A)); + Body *b_B = static_cast(b2Body_GetUserData(body_B)); + Entity *e_A = b_A ? b_A->GetEntity() : nullptr; + Entity *e_B = b_B ? b_B->GetEntity() : nullptr; + + if (e_A == model_) { + entity = e_B; + this_body = body_A; + other_body = body_B; + } else if (e_B == model_) { + entity = e_A; + this_body = body_B; + other_body = body_A; + } else { + return false; + } + return true; +} + +bool ModelPlugin::FilterContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + b2BodyId b1, b2; + Entity *e; + return FilterContact(shapeIdA, shapeIdB, e, b1, b2); +} + +}; // namespace flatland_server diff --git a/flatland_server/src/plugin_manager.cpp b/flatland_server/src/plugin_manager.cpp index 9904d30b..dab59306 100644 --- a/flatland_server/src/plugin_manager.cpp +++ b/flatland_server/src/plugin_manager.cpp @@ -1,248 +1,243 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name plugin_manager.cpp - * @brief Implementation for plugin manager - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -PluginManager::PluginManager() { - model_plugin_loader_ = - new pluginlib::ClassLoader( - "flatland_server", "flatland_server::ModelPlugin"); - world_plugin_loader_ = - new pluginlib::ClassLoader( - "flatland_server", "flatland_server::WorldPlugin"); -} - -PluginManager::~PluginManager() { - for (unsigned int i = 0; i < model_plugins_.size(); i++) { - model_plugins_[i].reset(); // deletes a shared pointer - } - for (unsigned int j = 0; j < world_plugins_.size(); j++) { - world_plugins_[j].reset(); // deletes a shared pointer - } - - delete model_plugin_loader_; - delete world_plugin_loader_; -} - -void PluginManager::BeforePhysicsStep(const Timekeeper &timekeeper_) { - for (const auto &model_plugin : model_plugins_) { - model_plugin->BeforePhysicsStep(timekeeper_); - } - for (const auto &world_plugin : world_plugins_) { - world_plugin->BeforePhysicsStep(timekeeper_); - } -} - -void PluginManager::AfterPhysicsStep(const Timekeeper &timekeeper_) { - for (const auto &model_plugin : model_plugins_) { - model_plugin->AfterPhysicsStep(timekeeper_); - } - for (const auto &world_plugin : world_plugins_) { - world_plugin->AfterPhysicsStep(timekeeper_); - } -} - -void PluginManager::DeleteModelPlugin(Model *model) { - model_plugins_.erase( - std::remove_if(model_plugins_.begin(), model_plugins_.end(), - [&](boost::shared_ptr p) { - return p->GetModel() == model; - }), - model_plugins_.end()); -} - -void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) { - std::string name = plugin_reader.Get("name"); - std::string type = plugin_reader.Get("type"); - - // ensure no plugin with the same model and name - if (std::count_if(model_plugins_.begin(), model_plugins_.end(), - [&](boost::shared_ptr i) { - return i->GetName() == name && i->GetModel() == model; - }) >= 1) { - throw YAMLException("Invalid \"plugins\" in " + Q(model->name_) + - " model, plugin with name " + Q(name) + - " already exists"); - } - - try { - if (!plugin_reader.Get("enabled", "true")) { - ROS_WARN_STREAM("Plugin " - << Q(model->name_) << "." - << plugin_reader.Get("name", "unnamed") - << " disabled"); - return; - } - } catch (...) { - ROS_WARN_STREAM("Body " << Q(model->name_) << "." - << plugin_reader.Get("name", "unnamed") - << " enabled because flag failed to parse: " - << plugin_reader.Get("enabled")); - } - - // remove the name, type and enabled of the YAML Node, the plugin does not - // need to know - // about these parameters, remove method is broken in yaml cpp 5.2, so we - // create a new node and add everything - YAML::Node yaml_node; - for (const auto &k : plugin_reader.Node()) { - if (k.first.as() != "name" && - k.first.as() != "type" && - k.first.as() != "enabled") { - yaml_node[k.first] = k.second; - } - } - - boost::shared_ptr model_plugin; - - std::string msg = "Model Plugin " + Q(name) + " type " + Q(type) + " model " + - Q(model->name_); - - try { - if (type.find("::") != std::string::npos) { - model_plugin = model_plugin_loader_->createInstance(type); - } else { - model_plugin = - model_plugin_loader_->createInstance("flatland_plugins::" + type); - } - } catch (pluginlib::PluginlibException &e) { - throw PluginException(msg + ": " + std::string(e.what())); - } - - try { - model_plugin->Initialize(type, name, model, yaml_node); - } catch (const std::exception &e) { - throw PluginException(msg + ": " + std::string(e.what())); - } - model_plugins_.push_back(model_plugin); - - ROS_INFO_NAMED("PluginManager", "%s loaded", msg.c_str()); -} - -void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, - YamlReader &world_config) { - std::string name = plugin_reader.Get("name"); - std::string type = plugin_reader.Get("type"); - ROS_INFO_NAMED("PluginManager", "finished load name and type"); - // first check for duplicate plugins - for (auto &it : world_plugins_) { - if (it->GetName() == name && it->GetType() == type) { - throw YAMLException("Invalid \"world plugins\" with name " + Q(name) + - ", type " + Q(type) + " already exists"); - } - } - - boost::shared_ptr world_plugin; - std::string msg = "World Plugin " + Q(name) + " type " + Q(type); - - YAML::Node yaml_node; - for (const auto &k : plugin_reader.Node()) { - if (k.first.as() != "name" && - k.first.as() != "type") { - yaml_node[k.first] = k.second; - } - } - - // try to create the instance - try { - if (type.find("::") != std::string::npos) { - world_plugin = world_plugin_loader_->createInstance(type); - } else { - world_plugin = - world_plugin_loader_->createInstance("flatland_plugins::" + type); - } - } catch (pluginlib::PluginlibException &e) { - throw PluginException(msg + ": " + std::string(e.what())); - } - - ROS_INFO_NAMED("PluginManager", "create instance finished"); - - try { - world_plugin->Initialize(world, name, type, yaml_node, world_config); - } catch (const std::exception &e) { - ROS_INFO_NAMED("PluginManager", "exception happened!"); - throw PluginException(msg + ": " + std::string(e.what())); - } - world_plugins_.push_back(world_plugin); - - ROS_INFO_NAMED("PluginManager", "%s loaded ", msg.c_str()); -} - -void PluginManager::BeginContact(b2Contact *contact) { - for (auto &model_plugin : model_plugins_) { - model_plugin->BeginContact(contact); - } -} - -void PluginManager::EndContact(b2Contact *contact) { - for (auto &model_plugin : model_plugins_) { - model_plugin->EndContact(contact); - } -} - -void PluginManager::PreSolve(b2Contact *contact, - const b2Manifold *oldManifold) { - for (auto &model_plugin : model_plugins_) { - model_plugin->PreSolve(contact, oldManifold); - } -} - -void PluginManager::PostSolve(b2Contact *contact, - const b2ContactImpulse *impulse) { - for (auto &model_plugin : model_plugins_) { - model_plugin->PostSolve(contact, impulse); - } -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name plugin_manager.cpp + * @brief Implementation for plugin manager + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +PluginManager::PluginManager() { + model_plugin_loader_ = + new pluginlib::ClassLoader( + "flatland_server", "flatland_server::ModelPlugin"); + world_plugin_loader_ = + new pluginlib::ClassLoader( + "flatland_server", "flatland_server::WorldPlugin"); +} + +PluginManager::~PluginManager() { + for (unsigned int i = 0; i < model_plugins_.size(); i++) { + model_plugins_[i].reset(); // deletes a shared pointer + } + for (unsigned int j = 0; j < world_plugins_.size(); j++) { + world_plugins_[j].reset(); // deletes a shared pointer + } + + delete model_plugin_loader_; + delete world_plugin_loader_; +} + +void PluginManager::BeforePhysicsStep(const Timekeeper &timekeeper_) { + for (const auto &model_plugin : model_plugins_) { + model_plugin->BeforePhysicsStep(timekeeper_); + } + for (const auto &world_plugin : world_plugins_) { + world_plugin->BeforePhysicsStep(timekeeper_); + } +} + +void PluginManager::AfterPhysicsStep(const Timekeeper &timekeeper_) { + for (const auto &model_plugin : model_plugins_) { + model_plugin->AfterPhysicsStep(timekeeper_); + } + for (const auto &world_plugin : world_plugins_) { + world_plugin->AfterPhysicsStep(timekeeper_); + } +} + +void PluginManager::DeleteModelPlugin(Model *model) { + model_plugins_.erase( + std::remove_if(model_plugins_.begin(), model_plugins_.end(), + [&](boost::shared_ptr p) { + return p->GetModel() == model; + }), + model_plugins_.end()); +} + +void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) { + std::string name = plugin_reader.Get("name"); + std::string type = plugin_reader.Get("type"); + + // ensure no plugin with the same model and name + if (std::count_if(model_plugins_.begin(), model_plugins_.end(), + [&](boost::shared_ptr i) { + return i->GetName() == name && i->GetModel() == model; + }) >= 1) { + throw YAMLException("Invalid \"plugins\" in " + Q(model->name_) + + " model, plugin with name " + Q(name) + + " already exists"); + } + + try { + if (!plugin_reader.Get("enabled", "true")) { + ROS_WARN_STREAM("Plugin " + << Q(model->name_) << "." + << plugin_reader.Get("name", "unnamed") + << " disabled"); + return; + } + } catch (...) { + ROS_WARN_STREAM("Body " << Q(model->name_) << "." + << plugin_reader.Get("name", "unnamed") + << " enabled because flag failed to parse: " + << plugin_reader.Get("enabled")); + } + + // remove the name, type and enabled of the YAML Node, the plugin does not + // need to know + // about these parameters, remove method is broken in yaml cpp 5.2, so we + // create a new node and add everything + YAML::Node yaml_node; + for (const auto &k : plugin_reader.Node()) { + if (k.first.as() != "name" && + k.first.as() != "type" && + k.first.as() != "enabled") { + yaml_node[k.first] = k.second; + } + } + + boost::shared_ptr model_plugin; + + std::string msg = "Model Plugin " + Q(name) + " type " + Q(type) + " model " + + Q(model->name_); + + try { + if (type.find("::") != std::string::npos) { + model_plugin = model_plugin_loader_->createInstance(type); + } else { + model_plugin = + model_plugin_loader_->createInstance("flatland_plugins::" + type); + } + } catch (pluginlib::PluginlibException &e) { + throw PluginException(msg + ": " + std::string(e.what())); + } + + try { + model_plugin->Initialize(type, name, model, yaml_node); + } catch (const std::exception &e) { + throw PluginException(msg + ": " + std::string(e.what())); + } + model_plugins_.push_back(model_plugin); + + ROS_INFO_NAMED("PluginManager", "%s loaded", msg.c_str()); +} + +void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, + YamlReader &world_config) { + std::string name = plugin_reader.Get("name"); + std::string type = plugin_reader.Get("type"); + ROS_INFO_NAMED("PluginManager", "finished load name and type"); + // first check for duplicate plugins + for (auto &it : world_plugins_) { + if (it->GetName() == name && it->GetType() == type) { + throw YAMLException("Invalid \"world plugins\" with name " + Q(name) + + ", type " + Q(type) + " already exists"); + } + } + + boost::shared_ptr world_plugin; + std::string msg = "World Plugin " + Q(name) + " type " + Q(type); + + YAML::Node yaml_node; + for (const auto &k : plugin_reader.Node()) { + if (k.first.as() != "name" && + k.first.as() != "type") { + yaml_node[k.first] = k.second; + } + } + + // try to create the instance + try { + if (type.find("::") != std::string::npos) { + world_plugin = world_plugin_loader_->createInstance(type); + } else { + world_plugin = + world_plugin_loader_->createInstance("flatland_plugins::" + type); + } + } catch (pluginlib::PluginlibException &e) { + throw PluginException(msg + ": " + std::string(e.what())); + } + + ROS_INFO_NAMED("PluginManager", "create instance finished"); + + try { + world_plugin->Initialize(world, name, type, yaml_node, world_config); + } catch (const std::exception &e) { + ROS_INFO_NAMED("PluginManager", "exception happened!"); + throw PluginException(msg + ": " + std::string(e.what())); + } + world_plugins_.push_back(world_plugin); + + ROS_INFO_NAMED("PluginManager", "%s loaded ", msg.c_str()); +} + +void PluginManager::BeginContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + for (auto &model_plugin : model_plugins_) { + model_plugin->BeginContact(shapeIdA, shapeIdB); + } +} + +void PluginManager::EndContact(b2ShapeId shapeIdA, b2ShapeId shapeIdB) { + for (auto &model_plugin : model_plugins_) { + model_plugin->EndContact(shapeIdA, shapeIdB); + } +} + +void PluginManager::OnContactHit(b2ShapeId shapeIdA, b2ShapeId shapeIdB, + b2Vec2 point, b2Vec2 normal, + float approachSpeed) { + for (auto &model_plugin : model_plugins_) { + model_plugin->OnContactHit(shapeIdA, shapeIdB, point, normal, + approachSpeed); + } +} + +}; // namespace flatland_server diff --git a/flatland_server/src/service_manager.cpp b/flatland_server/src/service_manager.cpp index 56643a6d..4810fcd0 100644 --- a/flatland_server/src/service_manager.cpp +++ b/flatland_server/src/service_manager.cpp @@ -1,167 +1,167 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_spawner.h - * @brief Definition for model spawner - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -namespace flatland_server { - -ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) - : world_(world), sim_man_(sim_man) { - ros::NodeHandle nh; - - spawn_model_service_ = - nh.advertiseService("spawn_model", &ServiceManager::SpawnModel, this); - delete_model_service_ = - nh.advertiseService("delete_model", &ServiceManager::DeleteModel, this); - move_model_service_ = - nh.advertiseService("move_model", &ServiceManager::MoveModel, this); - pause_service_ = nh.advertiseService("pause", &ServiceManager::Pause, this); - resume_service_ = - nh.advertiseService("resume", &ServiceManager::Resume, this); - toggle_pause_service_ = - nh.advertiseService("toggle_pause", &ServiceManager::TogglePause, this); - - if (spawn_model_service_) { - ROS_INFO_NAMED("Service Manager", "Model spawning service ready to go"); - } else { - ROS_ERROR_NAMED("Service Manager", "Error starting model spawning service"); - } - - if (delete_model_service_) { - ROS_INFO_NAMED("Service Manager", "Model deleting service ready to go"); - } else { - ROS_ERROR_NAMED("Service Manager", "Error starting model deleting service"); - } - - if (move_model_service_) { - ROS_INFO_NAMED("Service Manager", "Model moving service ready to go"); - } else { - ROS_ERROR_NAMED("Service Manager", "Error starting model moving service"); - } -} - -bool ServiceManager::SpawnModel(flatland_msgs::SpawnModel::Request &request, - flatland_msgs::SpawnModel::Response &response) { - ROS_DEBUG_NAMED("ServiceManager", - "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " - "name(\'%s\"), pose(%f,%f,%f)", - request.yaml_path.c_str(), request.ns.c_str(), - request.name.c_str(), request.pose.x, request.pose.y, - request.pose.theta); - - Pose pose(request.pose.x, request.pose.y, request.pose.theta); - - try { - world_->LoadModel(request.yaml_path, request.ns, request.name, pose); - response.success = true; - response.message = ""; - } catch (const std::exception &e) { - response.success = false; - response.message = std::string(e.what()); - ROS_ERROR_NAMED("ServiceManager", "Failed to load model! Exception: %s", - e.what()); - } - - return true; -} - -bool ServiceManager::DeleteModel( - flatland_msgs::DeleteModel::Request &request, - flatland_msgs::DeleteModel::Response &response) { - ROS_DEBUG_NAMED("ServiceManager", "Model delete requested with name(\"%s\")", - request.name.c_str()); - - try { - world_->DeleteModel(request.name); - response.success = true; - response.message = ""; - } catch (const std::exception &e) { - response.success = false; - response.message = std::string(e.what()); - } - - return true; -} - -bool ServiceManager::MoveModel(flatland_msgs::MoveModel::Request &request, - flatland_msgs::MoveModel::Response &response) { - ROS_DEBUG_NAMED("ServiceManager", "Model move requested with name(\"%s\")", - request.name.c_str()); - - Pose pose(request.pose.x, request.pose.y, request.pose.theta); - - try { - world_->MoveModel(request.name, pose); - response.success = true; - response.message = ""; - } catch (const std::exception &e) { - response.success = false; - response.message = std::string(e.what()); - } - - return true; -} - -bool ServiceManager::Pause(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response) { - world_->Pause(); - return true; -} - -bool ServiceManager::Resume(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response) { - world_->Resume(); - return true; -} - -bool ServiceManager::TogglePause(std_srvs::Empty::Request &request, - std_srvs::Empty::Response &response) { - world_->TogglePaused(); - return true; -} -}; +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_spawner.h + * @brief Definition for model spawner + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace flatland_server { + +ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) + : world_(world), sim_man_(sim_man) { + ros::NodeHandle nh; + + spawn_model_service_ = + nh.advertiseService("spawn_model", &ServiceManager::SpawnModel, this); + delete_model_service_ = + nh.advertiseService("delete_model", &ServiceManager::DeleteModel, this); + move_model_service_ = + nh.advertiseService("move_model", &ServiceManager::MoveModel, this); + pause_service_ = nh.advertiseService("pause", &ServiceManager::Pause, this); + resume_service_ = + nh.advertiseService("resume", &ServiceManager::Resume, this); + toggle_pause_service_ = + nh.advertiseService("toggle_pause", &ServiceManager::TogglePause, this); + + if (spawn_model_service_) { + ROS_INFO_NAMED("Service Manager", "Model spawning service ready to go"); + } else { + ROS_ERROR_NAMED("Service Manager", "Error starting model spawning service"); + } + + if (delete_model_service_) { + ROS_INFO_NAMED("Service Manager", "Model deleting service ready to go"); + } else { + ROS_ERROR_NAMED("Service Manager", "Error starting model deleting service"); + } + + if (move_model_service_) { + ROS_INFO_NAMED("Service Manager", "Model moving service ready to go"); + } else { + ROS_ERROR_NAMED("Service Manager", "Error starting model moving service"); + } +} + +bool ServiceManager::SpawnModel(flatland_msgs::SpawnModel::Request &request, + flatland_msgs::SpawnModel::Response &response) { + ROS_DEBUG_NAMED("ServiceManager", + "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " + "name(\'%s\"), pose(%f,%f,%f)", + request.yaml_path.c_str(), request.ns.c_str(), + request.name.c_str(), request.pose.x, request.pose.y, + request.pose.theta); + + Pose pose(request.pose.x, request.pose.y, request.pose.theta); + + try { + world_->LoadModel(request.yaml_path, request.ns, request.name, pose); + response.success = true; + response.message = ""; + } catch (const std::exception &e) { + response.success = false; + response.message = std::string(e.what()); + ROS_ERROR_NAMED("ServiceManager", "Failed to load model! Exception: %s", + e.what()); + } + + return true; +} + +bool ServiceManager::DeleteModel( + flatland_msgs::DeleteModel::Request &request, + flatland_msgs::DeleteModel::Response &response) { + ROS_DEBUG_NAMED("ServiceManager", "Model delete requested with name(\"%s\")", + request.name.c_str()); + + try { + world_->DeleteModel(request.name); + response.success = true; + response.message = ""; + } catch (const std::exception &e) { + response.success = false; + response.message = std::string(e.what()); + } + + return true; +} + +bool ServiceManager::MoveModel(flatland_msgs::MoveModel::Request &request, + flatland_msgs::MoveModel::Response &response) { + ROS_DEBUG_NAMED("ServiceManager", "Model move requested with name(\"%s\")", + request.name.c_str()); + + Pose pose(request.pose.x, request.pose.y, request.pose.theta); + + try { + world_->MoveModel(request.name, pose); + response.success = true; + response.message = ""; + } catch (const std::exception &e) { + response.success = false; + response.message = std::string(e.what()); + } + + return true; +} + +bool ServiceManager::Pause(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response) { + world_->Pause(); + return true; +} + +bool ServiceManager::Resume(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response) { + world_->Resume(); + return true; +} + +bool ServiceManager::TogglePause(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response) { + world_->TogglePaused(); + return true; +} +}; diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp index 862e4985..778a51bf 100644 --- a/flatland_server/src/simulation_manager.cpp +++ b/flatland_server/src/simulation_manager.cpp @@ -1,151 +1,184 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name simulation_manager.cpp - * @brief Simulation manager runs the physics+event loop - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/simulation_manager.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -SimulationManager::SimulationManager(std::string world_yaml_file, - double update_rate, double step_size, - bool show_viz, double viz_pub_rate) - : world_(nullptr), - update_rate_(update_rate), - step_size_(step_size), - show_viz_(show_viz), - viz_pub_rate_(viz_pub_rate), - world_yaml_file_(world_yaml_file) { - ROS_INFO_NAMED("SimMan", - "Simulation params: world_yaml_file(%s) update_rate(%f), " - "step_size(%f) show_viz(%s), viz_pub_rate(%f)", - world_yaml_file_.c_str(), update_rate_, step_size_, - show_viz_ ? "true" : "false", viz_pub_rate_); -} - -void SimulationManager::Main() { - ROS_INFO_NAMED("SimMan", "Initializing..."); - run_simulator_ = true; - - try { - world_ = World::MakeWorld(world_yaml_file_); - ROS_INFO_NAMED("SimMan", "World loaded"); - } catch (const std::exception& e) { - ROS_FATAL_NAMED("SimMan", "%s", e.what()); - return; - } - - if (show_viz_) world_->DebugVisualize(); - - int iterations = 0; - double filtered_cycle_util = 0; - double min_cycle_util = std::numeric_limits::infinity(); - double max_cycle_util = 0; - double viz_update_period = 1.0f / viz_pub_rate_; - ServiceManager service_manager(this, world_); - Timekeeper timekeeper; - - ros::WallRate rate(update_rate_); - timekeeper.SetMaxStepSize(step_size_); - ROS_INFO_NAMED("SimMan", "Simulation loop started"); - - while (ros::ok() && run_simulator_) { - // for updating visualization at a given rate - // see flatland_plugins/update_timer.cpp for this formula - double f = 0.0; - try { - f = fmod(ros::WallTime::now().toSec() + - (rate.expectedCycleTime().toSec() / 2.0), - viz_update_period); - } catch (std::runtime_error& ex) { - ROS_ERROR("Flatland runtime error: [%s]", ex.what()); - } - bool update_viz = ((f >= 0.0) && (f < rate.expectedCycleTime().toSec())); - - world_->Update(timekeeper); // Step physics by ros cycle time - - if (show_viz_ && update_viz) { - world_->DebugVisualize(false); // no need to update layer - DebugVisualization::Get().Publish( - timekeeper); // publish debug visualization - } - - ros::spinOnce(); - rate.sleep(); - - iterations++; - - double cycle_time = rate.cycleTime().toSec() * 1000; - double expected_cycle_time = rate.expectedCycleTime().toSec() * 1000; - double cycle_util = cycle_time / expected_cycle_time * 100; // in percent - double factor = timekeeper.GetStepSize() * 1000 / expected_cycle_time; - min_cycle_util = std::min(cycle_util, min_cycle_util); - if (iterations > 10) max_cycle_util = std::max(cycle_util, max_cycle_util); - filtered_cycle_util = 0.99 * filtered_cycle_util + 0.01 * cycle_util; - - ROS_INFO_THROTTLE_NAMED( - 1, "SimMan", - "utilization: min %.1f%% max %.1f%% ave %.1f%% factor: %.1f", - min_cycle_util, max_cycle_util, filtered_cycle_util, factor); - } - ROS_INFO_NAMED("SimMan", "Simulation loop ended"); - - delete world_; -} - -void SimulationManager::Shutdown() { - ROS_INFO_NAMED("SimMan", "Shutdown called"); - run_simulator_ = false; -} - -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name simulation_manager.cpp + * @brief Simulation manager runs the physics+event loop + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/simulation_manager.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +SimulationManager::SimulationManager(std::string world_yaml_file, + std::string models_path, + std::string world_plugins_path, + double update_rate, double step_size, + bool show_viz, double viz_pub_rate) + : world_(nullptr), + update_rate_(update_rate), + step_size_(step_size), + show_viz_(show_viz), + viz_pub_rate_(viz_pub_rate), + world_yaml_file_(world_yaml_file), + models_path_(models_path), + world_plugins_path_(world_plugins_path) { + ROS_INFO_NAMED("SimMan", + "Simulation params: world_yaml_file(%s) models_path(%s) " + "world_plugins_path(%s) update_rate(%f), " + "step_size(%f) show_viz(%s), viz_pub_rate(%f)", + world_yaml_file_.c_str(), models_path_.c_str(), + world_plugins_path_.c_str(), update_rate_, step_size_, + show_viz_ ? "true" : "false", viz_pub_rate_); +} + +void SimulationManager::Main() { + ROS_INFO_NAMED("SimMan", "Initializing..."); + run_simulator_ = true; + + try { + world_ = + World::MakeWorld(world_yaml_file_, models_path_, world_plugins_path_); + ROS_INFO_NAMED("SimMan", "World loaded"); + } catch (const std::exception& e) { + ROS_FATAL_NAMED("SimMan", "%s", e.what()); + return; + } + service_manager_.reset(nullptr); + + Timekeeper timekeeper; + ros::WallRate rate(update_rate_); + timekeeper.SetMaxStepSize(step_size_); + + int iterations = 0; + double filtered_cycle_util = 0; + double min_cycle_util = std::numeric_limits::infinity(); + double max_cycle_util = 0; + double viz_update_period = timekeeper.GetMaxStepSize() / + rate.expectedCycleTime().toSec() / viz_pub_rate_; + + ROS_INFO_NAMED("SimMan", "Waiting for Map"); + while (ros::ok() && run_simulator_) { + try { + world_->LoadWorldEntities(); + if (show_viz_) { + world_->DebugVisualize(); + } + service_manager_ = + std::unique_ptr(new ServiceManager(this, world_)); + break; + } catch (const YAMLException& ex) { + std::string exception(ex.what()); + if (exception.find("File does not exist") == std::string::npos) { + throw; + } + ROS_DEBUG_STREAM_THROTTLE(5, "Tried to load world yaml file " + << world_yaml_file_); + } + + timekeeper.StepTime(); + rate.sleep(); + } + + ROS_INFO_NAMED("SimMan", "Received Map, Simulation Loop Started"); + while (ros::ok() && run_simulator_) { + START_PROFILE(timekeeper, "Total Iteration"); + // for updating visualization at a given rate + // see flatland_plugins/update_timer.cpp for this formula + double f = 0.0; + static double t_init_offset = timekeeper.GetSimTime().toSec(); + try { + f = fmod(timekeeper.GetSimTime().toSec() - t_init_offset + + (rate.expectedCycleTime().toSec() / 2.0), + viz_update_period); + } catch (std::runtime_error& ex) { + ROS_ERROR("Flatland runtime error: [%s]", ex.what()); + } + bool update_viz = ((f >= 0.0) && (f < rate.expectedCycleTime().toSec())); + + world_->Update(timekeeper); // Step physics by ros cycle time + + if (show_viz_ && update_viz) { + world_->DebugVisualize(false); // no need to update layer + DebugVisualization::Get().Publish( + timekeeper); // publish debug visualization + } + + ros::spinOnce(); + END_PROFILE(timekeeper, "Total Iteration"); + rate.sleep(); + + iterations++; + + double cycle_time = rate.cycleTime().toSec() * 1000; + double expected_cycle_time = rate.expectedCycleTime().toSec() * 1000; + double cycle_util = cycle_time / expected_cycle_time * 100; // in percent + double factor = timekeeper.GetStepSize() * 1000 / expected_cycle_time; + min_cycle_util = std::min(cycle_util, min_cycle_util); + if (iterations > 10) max_cycle_util = std::max(cycle_util, max_cycle_util); + filtered_cycle_util = 0.99 * filtered_cycle_util + 0.01 * cycle_util; + + ROS_INFO_THROTTLE_NAMED( + 1, "SimMan", + "utilization: min %.1f%% max %.1f%% ave %.1f%% factor: %.1f", + min_cycle_util, max_cycle_util, filtered_cycle_util, factor); + } + ROS_INFO_NAMED("SimMan", "Simulation loop ended"); + + delete world_; +} + +void SimulationManager::Shutdown() { + ROS_INFO_NAMED("SimMan", "Shutdown called"); + run_simulator_ = false; +} + +}; // namespace flatland_server diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp index 8b27b597..4d77fe19 100644 --- a/flatland_server/src/timekeeper.cpp +++ b/flatland_server/src/timekeeper.cpp @@ -1,79 +1,79 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name timekeeper.cpp - * @brief Used for simulation time keeping - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -namespace flatland_server { - -Timekeeper::Timekeeper() - : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") { - clock_pub_ = nh_.advertise(clock_topic_, 1); -} - -void Timekeeper::StepTime() { - time_ += ros::Duration(max_step_size_); - - UpdateRosClock(); -} - -void Timekeeper::UpdateRosClock() const { - rosgraph_msgs::Clock clock; - clock.clock = time_; - clock_pub_.publish(clock); -} - -void Timekeeper::SetMaxStepSize(double step_size) { - max_step_size_ = step_size; -} - -const ros::Time& Timekeeper::GetSimTime() const { return time_; } - -double Timekeeper::GetStepSize() const { return max_step_size_; } - -double Timekeeper::GetMaxStepSize() const { return max_step_size_; } - -}; // namespace flatland_server \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name timekeeper.cpp + * @brief Used for simulation time keeping + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +namespace flatland_server { + +Timekeeper::Timekeeper() + : time_(ros::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") { + clock_pub_ = nh_.advertise(clock_topic_, 1); +} + +void Timekeeper::StepTime() { + time_ += ros::Duration(max_step_size_); + + UpdateRosClock(); +} + +void Timekeeper::UpdateRosClock() const { + rosgraph_msgs::Clock clock; + clock.clock = time_; + clock_pub_.publish(clock); +} + +void Timekeeper::SetMaxStepSize(double step_size) { + max_step_size_ = step_size; +} + +const ros::Time& Timekeeper::GetSimTime() const { return time_; } + +double Timekeeper::GetStepSize() const { return max_step_size_; } + +double Timekeeper::GetMaxStepSize() const { return max_step_size_; } + +}; // namespace flatland_server diff --git a/flatland_server/src/world.cpp b/flatland_server/src/world.cpp index 0b316526..a4ddd9fd 100644 --- a/flatland_server/src/world.cpp +++ b/flatland_server/src/world.cpp @@ -1,354 +1,477 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world.cpp - * @brief Loads world file - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace flatland_server { - -World::World() - : gravity_(0, 0), - service_paused_(false), - int_marker_manager_(&models_, &plugin_manager_) { - physics_world_ = new b2World(gravity_); - physics_world_->SetContactListener(this); -} - -World::~World() { - ROS_INFO_NAMED("World", "Destroying world..."); - - // The order of things matters in the destructor. The contact listener is - // removed first to avoid the triggering the contact functions in plugin - // manager which might cause it to work with deleted layers/models. - physics_world_->SetContactListener(nullptr); - - // the physics body of layers are set to null because there are tons of - // fixtures in a layer and it is too slow for the destroyBody method to remove - // them since the AABB tree gets restructured everytime a fixture is removed - // The memory will later be freed by deleting the world - for (auto &layer : layers_) { - if (layer->body_ != nullptr) { - layer->body_->physics_body_ = nullptr; - } - delete layer; - } - - // The bodies of models are not set to null like layers because there aren't - // nearly as many fixtures, and we might hide some memory problems by using - // the shortcut - for (unsigned int i = 0; i < models_.size(); i++) { - delete models_[i]; - } - - // This frees the entire Box2D world with everything in it - delete physics_world_; - - ROS_INFO_NAMED("World", "World destroyed"); -} - -void World::Update(Timekeeper &timekeeper) { - if (!IsPaused()) { - plugin_manager_.BeforePhysicsStep(timekeeper); - physics_world_->Step(timekeeper.GetStepSize(), physics_velocity_iterations_, - physics_position_iterations_); - timekeeper.StepTime(); - plugin_manager_.AfterPhysicsStep(timekeeper); - } - int_marker_manager_.update(); -} - -void World::BeginContact(b2Contact *contact) { - plugin_manager_.BeginContact(contact); -} - -void World::EndContact(b2Contact *contact) { - plugin_manager_.EndContact(contact); -} - -void World::PreSolve(b2Contact *contact, const b2Manifold *oldManifold) { - plugin_manager_.PreSolve(contact, oldManifold); -} - -void World::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) { - plugin_manager_.PostSolve(contact, impulse); -} - -World *World::MakeWorld(const std::string &yaml_path) { - YamlReader world_reader = YamlReader(yaml_path); - YamlReader prop_reader = world_reader.Subnode("properties", YamlReader::MAP); - int v = prop_reader.Get("velocity_iterations", 10); - int p = prop_reader.Get("position_iterations", 10); - prop_reader.EnsureAccessedAllKeys(); - - World *w = new World(); - - w->world_yaml_dir_ = boost::filesystem::path(yaml_path).parent_path(); - w->physics_velocity_iterations_ = v; - w->physics_position_iterations_ = p; - - try { - YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST); - YamlReader models_reader = - world_reader.SubnodeOpt("models", YamlReader::LIST); - YamlReader world_plugin_reader = - world_reader.SubnodeOpt("plugins", YamlReader::LIST); - world_reader.EnsureAccessedAllKeys(); - w->LoadLayers(layers_reader); - w->LoadModels(models_reader); - w->LoadWorldPlugins(world_plugin_reader, w, world_reader); - } catch (const YAMLException &e) { - ROS_FATAL_NAMED("World", "Error loading from YAML"); - delete w; - throw e; - } catch (const PluginException &e) { - ROS_FATAL_NAMED("World", "Error loading plugins"); - delete w; - throw e; - } catch (const Exception &e) { - ROS_FATAL_NAMED("World", "Error loading world"); - delete w; - throw e; - } - return w; -} - -void World::LoadLayers(YamlReader &layers_reader) { - // loop through each layer and parse the data - for (int i = 0; i < layers_reader.NodeSize(); i++) { - YamlReader reader = layers_reader.Subnode(i, YamlReader::MAP); - YamlReader name_reader = reader.Subnode("name", YamlReader::NO_CHECK); - - // allow names to be either a just a string or a list of strings - std::vector names; - if (name_reader.Node().IsSequence()) { - names = name_reader.AsList(1, -1); - } else { - names.push_back(name_reader.As()); - } - - if (cfr_.LayersCount() + names.size() > cfr_.MAX_LAYERS) { - throw YAMLException( - "Unable to add " + std::to_string(names.size()) + - " additional layer(s) {" + boost::algorithm::join(names, ", ") + - "}, current layers count is " + std::to_string(cfr_.LayersCount()) + - ", max allowed is " + std::to_string(cfr_.MAX_LAYERS)); - } - - boost::filesystem::path map_path(reader.Get("map", "")); - Color color = reader.GetColor("color", Color(1, 1, 1, 1)); - auto properties = - reader.SubnodeOpt("properties", YamlReader::NodeTypeCheck::MAP).Node(); - reader.EnsureAccessedAllKeys(); - - for (const auto &name : names) { - if (cfr_.RegisterLayer(name) == cfr_.LAYER_ALREADY_EXIST) { - throw YAMLException("Layer with name " + Q(name) + " already exists"); - } - } - - if (map_path.string().front() != '/' && map_path.string().length() > 0) { - map_path = world_yaml_dir_ / map_path; - } - - ROS_INFO_NAMED("World", "Loading layer \"%s\" from path=\"%s\"", - names[0].c_str(), map_path.string().c_str()); - - Layer *layer = Layer::MakeLayer(physics_world_, &cfr_, map_path.string(), - names, color, properties); - layers_name_map_.insert( - std::pair, Layer *>(names, layer)); - layers_.push_back(layer); - - ROS_INFO_NAMED("World", "Layer \"%s\" loaded", layer->name_.c_str()); - layer->DebugOutput(); - } -} - -void World::LoadModels(YamlReader &models_reader) { - if (!models_reader.IsNodeNull()) { - for (int i = 0; i < models_reader.NodeSize(); i++) { - YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); - - std::string name = reader.Get("name"); - std::string ns = reader.Get("namespace", ""); - Pose pose = reader.GetPose("pose", Pose(0, 0, 0)); - std::string path = reader.Get("model"); - reader.EnsureAccessedAllKeys(); - LoadModel(path, ns, name, pose); - } - } -} - -void World::LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, - YamlReader &world_config) { - if (!world_plugin_reader.IsNodeNull()) { - for (int i = 0; i < world_plugin_reader.NodeSize(); i++) { - YamlReader reader = world_plugin_reader.Subnode(i, YamlReader::MAP); - ROS_INFO_NAMED("World", "loading world_plugin"); - plugin_manager_.LoadWorldPlugin(world, reader, world_config); - } - } -} -void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, - const std::string &name, const Pose &pose) { - // ensure no duplicate model names - if (std::count_if(models_.begin(), models_.end(), - [&](Model *m) { return m->name_ == name; }) >= 1) { - throw YAMLException("Model with name " + Q(name) + " already exists"); - } - - boost::filesystem::path abs_path(model_yaml_path); - if (model_yaml_path.front() != '/') { - abs_path = world_yaml_dir_ / abs_path; - } - - ROS_INFO_NAMED("World", "Loading model from path=\"%s\"", - abs_path.string().c_str()); - - Model *m = - Model::MakeModel(physics_world_, &cfr_, abs_path.string(), ns, name); - m->TransformAll(pose); - - try { - for (int i = 0; i < m->plugins_reader_.NodeSize(); i++) { - YamlReader plugin_reader = m->plugins_reader_.Subnode(i, YamlReader::MAP); - plugin_manager_.LoadModelPlugin(m, plugin_reader); - } - } catch (const YAMLException &e) { - plugin_manager_.DeleteModelPlugin(m); - delete m; - throw e; - } catch (const PluginException &e) { - plugin_manager_.DeleteModelPlugin(m); - delete m; - throw e; - } - - models_.push_back(m); - - visualization_msgs::MarkerArray body_markers; - for (size_t i = 0; i < m->bodies_.size(); i++) { - DebugVisualization::Get().BodyToMarkers( - body_markers, m->bodies_[i]->physics_body_, 1.0, 0.0, 0.0, 1.0); - } - int_marker_manager_.createInteractiveMarker(name, pose, body_markers); - - ROS_INFO_NAMED("World", "Model \"%s\" loaded", m->name_.c_str()); - m->DebugOutput(); -} - -void World::DeleteModel(const std::string &name) { - bool found = false; - - for (unsigned int i = 0; i < models_.size(); i++) { - // name is unique, so there will only be one object with this name - if (models_[i]->GetName() == name) { - // delete the plugins associated with the model - plugin_manager_.DeleteModelPlugin(models_[i]); - delete models_[i]; - models_.erase(models_.begin() + i); - int_marker_manager_.deleteInteractiveMarker(name); - found = true; - break; - } - } - - if (!found) { - throw Exception("Flatland World: failed to delete model, model with name " + - Q(name) + " does not exist"); - } -} - -void World::MoveModel(const std::string &name, const Pose &pose) { - // Find desired model - bool found = false; - - for (unsigned int i = 0; i < models_.size(); i++) { - if (models_[i]->GetName() == name) { - // move the model - models_[i]->SetPose(pose); - found = true; - break; - } - } - - if (!found) { - throw Exception("Flatland World: failed to move model, model with name " + - Q(name) + " does not exist"); - } -} - -void World::Pause() { service_paused_ = true; } - -void World::Resume() { service_paused_ = false; } - -void World::TogglePaused() { service_paused_ = !service_paused_; } - -bool World::IsPaused() { - return service_paused_ || int_marker_manager_.isManipulating(); -} - -void World::DebugVisualize(bool update_layers) { - if (update_layers) { - for (const auto &layer : layers_) { - layer->DebugVisualize(); - } - } - - for (const auto &model : models_) { - model->DebugVisualize(); - } -} -}; // namespace flatland_server +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world.cpp + * @brief Loads world file + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace flatland_server { + +// --------------- enkiTS <-> Box2D v3 task adapter --------------- + +class FlatlandTask : public enki::ITaskSet { +public: + FlatlandTask() = default; + + void ExecuteRange(enki::TaskSetPartition range, + uint32_t threadIndex) override { + m_task(range.start, range.end, threadIndex, m_taskContext); + } + + b2TaskCallback *m_task = nullptr; + void *m_taskContext = nullptr; +}; + +static constexpr int kMaxTasks = 128; +static FlatlandTask s_tasks[kMaxTasks]; +static int s_taskCount = 0; + +static void *EnkiEnqueueTask(b2TaskCallback *fcn, int32_t itemCount, + int32_t minRange, void *taskContext, + void *userContext) { + auto *scheduler = static_cast(userContext); + if (s_taskCount < kMaxTasks) { + FlatlandTask &task = s_tasks[s_taskCount]; + task.m_SetSize = itemCount; + task.m_MinRange = minRange; + task.m_task = fcn; + task.m_taskContext = taskContext; + scheduler->AddTaskSetToPipe(&task); + ++s_taskCount; + return &task; + } + // Fallback: run inline if pool exhausted + fcn(0, itemCount, 0, taskContext); + return nullptr; +} + +static void EnkiFinishTask(void *userTask, void *userContext) { + if (userTask != nullptr) { + auto *scheduler = static_cast(userContext); + auto *task = static_cast(userTask); + scheduler->WaitforTask(task); + } +} + +// ---------------------------------------------------------------- + +World::World() + : gravity_({0.0f, 0.0f}), + service_paused_(false), + int_marker_manager_(&models_, &plugin_manager_) { + task_scheduler_.Initialize(); + + b2WorldDef world_def = b2DefaultWorldDef(); + world_def.gravity = gravity_; + world_def.workerCount = task_scheduler_.GetNumTaskThreads(); + world_def.enqueueTask = EnkiEnqueueTask; + world_def.finishTask = EnkiFinishTask; + world_def.userTaskContext = &task_scheduler_; + world_id_ = b2CreateWorld(&world_def); +} + +World::~World() { + ROS_INFO_NAMED("World", "Destroying world..."); + + // The order of things matters in the destructor. The contact listener is + // removed first to avoid the triggering the contact functions in plugin + // manager which might cause it to work with deleted layers/models. + + // the physics body of layers are set to b2_nullBodyId because there are tons + // of shapes in a layer and it is too slow for the DestroyBody method to + // remove them since the AABB tree gets restructured every time a shape is + // removed. The memory will later be freed by destroying the world. + for (auto &layer : layers_) { + if (layer->body_ != nullptr) { + layer->body_->physics_body_ = b2_nullBodyId; + } + delete layer; + } + + // The bodies of models are not set to null like layers because there aren't + // nearly as many fixtures, and we might hide some memory problems by using + // the shortcut + for (unsigned int i = 0; i < models_.size(); i++) { + delete models_[i]; + } + + // This frees the entire Box2D world with everything in it + b2DestroyWorld(world_id_); + world_id_ = b2_nullWorldId; + task_scheduler_.WaitforAllAndShutdown(); + + ROS_INFO_NAMED("World", "World destroyed"); +} + +void World::Update(Timekeeper &timekeeper) { + if (!IsPaused()) { + plugin_manager_.BeforePhysicsStep(timekeeper); + s_taskCount = 0; // Reset task pool for this step + b2World_Step(world_id_, timekeeper.GetStepSize(), + physics_velocity_iterations_); + + // Poll contact events (replaces b2ContactListener callbacks from v2) + b2ContactEvents events = b2World_GetContactEvents(world_id_); + for (int i = 0; i < events.beginCount; i++) { + const b2ContactBeginTouchEvent &e = events.beginEvents[i]; + plugin_manager_.BeginContact(e.shapeIdA, e.shapeIdB); + } + for (int i = 0; i < events.endCount; i++) { + const b2ContactEndTouchEvent &e = events.endEvents[i]; + plugin_manager_.EndContact(e.shapeIdA, e.shapeIdB); + } + for (int i = 0; i < events.hitCount; i++) { + const b2ContactHitEvent &e = events.hitEvents[i]; + plugin_manager_.OnContactHit(e.shapeIdA, e.shapeIdB, e.point, e.normal, + e.approachSpeed); + } + + timekeeper.StepTime(); + plugin_manager_.AfterPhysicsStep(timekeeper); + } + int_marker_manager_.update(); +} + +World *World::MakeWorld(const std::string &yaml_path) { + YamlReader world_reader = YamlReader(yaml_path); + YamlReader prop_reader = world_reader.Subnode("properties", YamlReader::MAP); + int v = prop_reader.Get("velocity_iterations", 10); + int p = prop_reader.Get("position_iterations", 10); + prop_reader.EnsureAccessedAllKeys(); + + World *w = new World(); + + w->world_yaml_dir_ = boost::filesystem::path(yaml_path).parent_path(); + w->yaml_path_ = yaml_path; + w->physics_velocity_iterations_ = v; + + try { + YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST); + YamlReader models_reader = + world_reader.SubnodeOpt("models", YamlReader::LIST); + YamlReader world_plugin_reader = + world_reader.SubnodeOpt("plugins", YamlReader::LIST); + world_reader.EnsureAccessedAllKeys(); + w->LoadLayers(layers_reader); + w->LoadModels(models_reader); + w->LoadWorldPlugins(world_plugin_reader, w, world_reader); + } catch (const YAMLException &e) { + ROS_FATAL_NAMED("World", "Error loading from YAML"); + delete w; + throw e; + } catch (const PluginException &e) { + ROS_FATAL_NAMED("World", "Error loading plugins"); + delete w; + throw e; + } catch (const Exception &e) { + ROS_FATAL_NAMED("World", "Error loading world"); + delete w; + throw e; + } + return w; +} + +World *World::MakeWorld(const std::string &yaml_path, + const std::string &models_path, + const std::string &world_plugins_path) { + YamlReader world_settings_reader = YamlReader(world_plugins_path); + YamlReader prop_reader = + world_settings_reader.Subnode("properties", YamlReader::MAP); + YamlReader world_plugin_reader = + world_settings_reader.SubnodeOpt("plugins", YamlReader::LIST); + + int v = prop_reader.Get("velocity_iterations", 10); + int p = prop_reader.Get("position_iterations", 10); + prop_reader.EnsureAccessedAllKeys(); + + World *w = new World(); + + w->world_yaml_dir_ = boost::filesystem::path(yaml_path).parent_path(); + w->physics_velocity_iterations_ = v; + w->physics_position_iterations_ = p; + w->models_path_ = models_path; + w->yaml_path_ = yaml_path; + + try { + w->LoadWorldPlugins(world_plugin_reader, w, world_settings_reader); + } catch (const YAMLException &e) { + ROS_FATAL_NAMED("World", "Error loading world plugins"); + delete w; + throw e; + } catch (const PluginException &e) { + ROS_FATAL_NAMED("World", "Error loading plugins"); + delete w; + throw e; + } + return w; +} + +void World::LoadWorldEntities() { + try { + YamlReader map_info_reader = YamlReader(yaml_path_); + YamlReader layers_reader = + map_info_reader.Subnode("layers", YamlReader::LIST); + YamlReader models_reader = + map_info_reader.SubnodeOpt("models", YamlReader::LIST); + LoadLayers(layers_reader); + LoadModels(models_reader); + } catch (const YAMLException &e) { + ROS_WARN_STREAM_DELAYED_THROTTLE_NAMED(1, "World", + yaml_path_ << " not loaded yet"); + throw e; + } +} + +void World::SlowSimTime(const std::string & /*agent*/) { + // Stub: dynamic fast-sim-time feature not yet ported to Box2D v3 branch +} + +void World::FastSimTime(const std::string & /*agent*/) { + // Stub: dynamic fast-sim-time feature not yet ported to Box2D v3 branch +} + +void World::InitializeDynamicFastSim(double /*max_lower_speed*/, + double /*min_lower_speed*/, + int /*num_robots_threshold*/) { + // Stub: dynamic fast-sim initialization not yet ported to Box2D v3 branch +} + +void World::LoadLayers(YamlReader &layers_reader) { + // loop through each layer and parse the data + for (int i = 0; i < layers_reader.NodeSize(); i++) { + YamlReader reader = layers_reader.Subnode(i, YamlReader::MAP); + YamlReader name_reader = reader.Subnode("name", YamlReader::NO_CHECK); + + // allow names to be either a just a string or a list of strings + std::vector names; + if (name_reader.Node().IsSequence()) { + names = name_reader.AsList(1, -1); + } else { + names.push_back(name_reader.As()); + } + + if (cfr_.LayersCount() + names.size() > cfr_.MAX_LAYERS) { + throw YAMLException( + "Unable to add " + std::to_string(names.size()) + + " additional layer(s) {" + boost::algorithm::join(names, ", ") + + "}, current layers count is " + std::to_string(cfr_.LayersCount()) + + ", max allowed is " + std::to_string(cfr_.MAX_LAYERS)); + } + + boost::filesystem::path map_path(reader.Get("map", "")); + Color color = reader.GetColor("color", Color(1, 1, 1, 1)); + auto properties = + reader.SubnodeOpt("properties", YamlReader::NodeTypeCheck::MAP).Node(); + reader.EnsureAccessedAllKeys(); + + for (const auto &name : names) { + if (cfr_.RegisterLayer(name) == cfr_.LAYER_ALREADY_EXIST) { + throw YAMLException("Layer with name " + Q(name) + " already exists"); + } + } + + if (map_path.string().front() != '/' && map_path.string().length() > 0) { + map_path = world_yaml_dir_ / map_path; + } + + ROS_INFO_NAMED("World", "Loading layer \"%s\" from path=\"%s\"", + names[0].c_str(), map_path.string().c_str()); + + Layer *layer = Layer::MakeLayer(world_id_, &cfr_, map_path.string(), + names, color, properties); + layers_name_map_.insert( + std::pair, Layer *>(names, layer)); + layers_.push_back(layer); + + ROS_INFO_NAMED("World", "Layer \"%s\" loaded", layer->name_.c_str()); + layer->DebugOutput(); + } +} + +void World::LoadModels(YamlReader &models_reader) { + if (!models_reader.IsNodeNull()) { + for (int i = 0; i < models_reader.NodeSize(); i++) { + YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); + + std::string name = reader.Get("name"); + std::string ns = reader.Get("namespace", ""); + Pose pose = reader.GetPose("pose", Pose(0, 0, 0)); + std::string path = reader.Get("model"); + reader.EnsureAccessedAllKeys(); + LoadModel(path, ns, name, pose); + } + } +} + +void World::LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, + YamlReader &world_config) { + if (!world_plugin_reader.IsNodeNull()) { + for (int i = 0; i < world_plugin_reader.NodeSize(); i++) { + YamlReader reader = world_plugin_reader.Subnode(i, YamlReader::MAP); + ROS_INFO_NAMED("World", "loading world_plugin"); + plugin_manager_.LoadWorldPlugin(world, reader, world_config); + } + } +} +void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, + const std::string &name, const Pose &pose) { + // ensure no duplicate model names + if (std::count_if(models_.begin(), models_.end(), + [&](Model *m) { return m->name_ == name; }) >= 1) { + throw YAMLException("Model with name " + Q(name) + " already exists"); + } + + boost::filesystem::path abs_path(model_yaml_path); + if (model_yaml_path.front() != '/') { + abs_path = world_yaml_dir_ / abs_path; + } + + ROS_INFO_NAMED("World", "Loading model from path=\"%s\"", + abs_path.string().c_str()); + + Model *m = + Model::MakeModel(this, world_id_, &cfr_, abs_path.string(), ns, name); + m->TransformAll(pose); + + try { + for (int i = 0; i < m->plugins_reader_.NodeSize(); i++) { + YamlReader plugin_reader = m->plugins_reader_.Subnode(i, YamlReader::MAP); + plugin_manager_.LoadModelPlugin(m, plugin_reader); + } + } catch (const YAMLException &e) { + plugin_manager_.DeleteModelPlugin(m); + delete m; + throw e; + } catch (const PluginException &e) { + plugin_manager_.DeleteModelPlugin(m); + delete m; + throw e; + } + + models_.push_back(m); + + visualization_msgs::MarkerArray body_markers; + for (size_t i = 0; i < m->bodies_.size(); i++) { + DebugVisualization::Get().BodyToMarkers( + body_markers, m->bodies_[i]->physics_body_, 1.0, 0.0, 0.0, 1.0); + } + int_marker_manager_.createInteractiveMarker(name, pose, body_markers); + + ROS_INFO_NAMED("World", "Model \"%s\" loaded", m->name_.c_str()); + m->DebugOutput(); +} + +void World::DeleteModel(const std::string &name) { + bool found = false; + + for (unsigned int i = 0; i < models_.size(); i++) { + // name is unique, so there will only be one object with this name + if (models_[i]->GetName() == name) { + // delete the plugins associated with the model + plugin_manager_.DeleteModelPlugin(models_[i]); + delete models_[i]; + models_.erase(models_.begin() + i); + int_marker_manager_.deleteInteractiveMarker(name); + found = true; + break; + } + } + + if (!found) { + throw Exception("Flatland World: failed to delete model, model with name " + + Q(name) + " does not exist"); + } +} + +void World::MoveModel(const std::string &name, const Pose &pose) { + // Find desired model + bool found = false; + + for (unsigned int i = 0; i < models_.size(); i++) { + if (models_[i]->GetName() == name) { + // move the model + models_[i]->SetPose(pose); + found = true; + break; + } + } + + if (!found) { + throw Exception("Flatland World: failed to move model, model with name " + + Q(name) + " does not exist"); + } +} + +void World::Pause() { service_paused_ = true; } + +void World::Resume() { service_paused_ = false; } + +void World::TogglePaused() { service_paused_ = !service_paused_; } + +bool World::IsPaused() { + return service_paused_ || int_marker_manager_.isManipulating(); +} + +void World::DebugVisualize(bool update_layers) { + if (update_layers) { + for (const auto &layer : layers_) { + layer->DebugVisualize(); + } + } + + for (const auto &model : models_) { + model->DebugVisualize(); + } +} +}; // namespace flatland_server diff --git a/flatland_server/src/world_plugin.cpp b/flatland_server/src/world_plugin.cpp index f1c61788..b9c65435 100644 --- a/flatland_server/src/world_plugin.cpp +++ b/flatland_server/src/world_plugin.cpp @@ -1,65 +1,65 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name world_plugin.cpp - * @brief Implent for WorldPlugin pluginlib plugins - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -#include -#include -#include - -namespace flatland_server { -void WorldPlugin::Initialize(World *world, std::string name, std::string type, - YAML::Node &plugin_reader, - YamlReader &world_config) { - world_ = world; - name_ = name; - type_ = type; - world_config_ = world_config; - plugin_type_ = PluginType::World; - OnInitialize(plugin_reader); -} -} \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name world_plugin.cpp + * @brief Implent for WorldPlugin pluginlib plugins + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include +#include +#include + +namespace flatland_server { +void WorldPlugin::Initialize(World *world, std::string name, std::string type, + YAML::Node &plugin_reader, + YamlReader &world_config) { + world_ = world; + name_ = name; + type_ = type; + world_config_ = world_config; + plugin_type_ = PluginType::World; + OnInitialize(plugin_reader); +} +} diff --git a/flatland_server/src/yaml_preprocessor.cpp b/flatland_server/src/yaml_preprocessor.cpp index b35f99f9..feb110fb 100644 --- a/flatland_server/src/yaml_preprocessor.cpp +++ b/flatland_server/src/yaml_preprocessor.cpp @@ -1,217 +1,217 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2018 Avidbots Corp. - * @name yaml_preprocessor - * @brief Yaml preprocessor using Lua - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/yaml_preprocessor.h" -#include -#include -#include - -#include -#include - -namespace flatland_server { - -void YamlPreprocessor::Parse(YAML::Node &node) { - YamlPreprocessor::ProcessNodes(node); -} - -void YamlPreprocessor::ProcessNodes(YAML::Node &node) { - switch (node.Type()) { - case YAML::NodeType::Sequence: - for (YAML::Node child : node) { - YamlPreprocessor::ProcessNodes(child); - } - break; - case YAML::NodeType::Map: - for (YAML::iterator it = node.begin(); it != node.end(); ++it) { - YamlPreprocessor::ProcessNodes(it->second); - } - break; - case YAML::NodeType::Scalar: - if (node.as().compare(0, 5, "$eval") == 0) { - ProcessScalarNode(node); - } - break; - default: - ROS_DEBUG_STREAM( - "Yaml Preprocessor found an unexpected type: " << node.Type()); - break; - } -} - -void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { - std::string value = node.as().substr(5); // omit the $parse - boost::algorithm::trim(value); // trim whitespace - ROS_INFO_STREAM("Attempting to parse lua " << value); - - if (value.find("return ") == std::string::npos) { // Has no return statement - value = "return " + value; - } - - // Create the Lua context - lua_State *L = luaL_newstate(); - luaL_openlibs(L); - lua_pushcfunction(L, YamlPreprocessor::LuaGetEnv); - lua_setglobal(L, "env"); - lua_pushcfunction(L, YamlPreprocessor::LuaGetParam); - lua_setglobal(L, "param"); - - try { /* Attempt to run the Lua string and parse its results */ - int error = luaL_dostring(L, value.c_str()); - if (error) { - ROS_ERROR_STREAM(lua_tostring(L, -1)); - lua_pop(L, 1); /* pop error message from the stack */ - } else { - int t = lua_type(L, 1); - if (t == LUA_TNIL) { - node = ""; - ROS_INFO_STREAM("Preprocessor parsed " << value << " as empty string"); - } else if (t == LUA_TBOOLEAN) { - ROS_INFO_STREAM("Preprocessor parsed " - << value << " as bool " - << (lua_toboolean(L, 1) ? "true" : "false")); - node = lua_toboolean(L, 1) ? "true" : "false"; - } else if (t == LUA_TSTRING || t == LUA_TNUMBER) { - ROS_INFO_STREAM("Preprocessor parsed " << value << " as " - << lua_tostring(L, 1)); - node = lua_tostring(L, 1); - } else { - ROS_ERROR_STREAM("No lua output for " << value); - } - } - } catch ( - ...) { /* Something went wrong parsing the lua, or gettings its results */ - ROS_ERROR_STREAM("Lua error in: " << value); - } -} - -YAML::Node YamlPreprocessor::LoadParse(const std::string &path) { - YAML::Node node; - - try { - node = YAML::LoadFile(path); - } catch (const YAML::BadFile &e) { - throw YAMLException("File does not exist, path=" + path); - } catch (const YAML::ParserException &e) { - throw YAMLException("Malformatted file, path=" + path, e); - } catch (const YAML::Exception &e) { - throw YAMLException("Error loading file, path=" + path, e); - } - - YamlPreprocessor::Parse(node); - return node; -} - -int YamlPreprocessor::LuaGetEnv(lua_State *L) { - const char *name = lua_tostring(L, 1); - const char *env = std::getenv(name); - - if (lua_gettop(L) == 2 && env == NULL) { // use default - if (lua_isnumber(L, 2)) { - lua_pushnumber(L, lua_tonumber(L, 2)); - } else if (lua_isstring(L, 2)) { - lua_pushstring(L, lua_tostring(L, 2)); - } else if (lua_isboolean(L, 2)) { - lua_pushboolean(L, lua_toboolean(L, 2)); - } - } else { // no default - if (env == NULL) { // Push back a nil - ROS_WARN_STREAM("No environment variable for: " << name); - lua_pushnil(L); - } else { - ROS_WARN_STREAM("Found env for " << name); - try { // Try to push a number - double x = boost::lexical_cast(env); - lua_pushnumber(L, x); - } catch (boost::bad_lexical_cast &) { // Otherwise it's a string - lua_pushstring(L, env); - } - } - } - - return 1; // 1 return value -} - -int YamlPreprocessor::LuaGetParam(lua_State *L) { - const char *name = lua_tostring(L, 1); - std::string param_s; - double param_d; - bool param_b; - - if (lua_gettop(L) == 2 && !ros::param::has(name)) { // use default - if (lua_isnumber(L, 2)) { - lua_pushnumber(L, lua_tonumber(L, 2)); - } else if (lua_isboolean(L, 2)) { - lua_pushboolean(L, lua_toboolean(L, 2)); - } else if (lua_isstring(L, 2)) { - lua_pushstring(L, lua_tostring(L, 2)); - } else { - ROS_WARN_STREAM("Couldn't load int/double/string value at param " - << name); - lua_pushnil(L); - } - } else { // no default - if (!ros::param::has(name)) { // Push back a nil - ROS_WARN_STREAM("No rosparam found for: " << name); - lua_pushnil(L); - } else { - if (ros::param::get(name, param_d)) { - lua_pushnumber(L, param_d); - } else if (ros::param::get(name, param_s)) { - lua_pushstring(L, param_s.c_str()); - } else if (ros::param::get(name, param_b)) { - lua_pushstring(L, param_b ? "true" : "false"); - } else { - ROS_WARN_STREAM("Couldn't load int/double/string value at param " - << name); - lua_pushnil(L); - } - } - } - - return 1; // 1 return value -} -} \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2018 Avidbots Corp. + * @name yaml_preprocessor + * @brief Yaml preprocessor using Lua + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/yaml_preprocessor.h" +#include +#include +#include + +#include +#include + +namespace flatland_server { + +void YamlPreprocessor::Parse(YAML::Node &node) { + YamlPreprocessor::ProcessNodes(node); +} + +void YamlPreprocessor::ProcessNodes(YAML::Node &node) { + switch (node.Type()) { + case YAML::NodeType::Sequence: + for (YAML::Node child : node) { + YamlPreprocessor::ProcessNodes(child); + } + break; + case YAML::NodeType::Map: + for (YAML::iterator it = node.begin(); it != node.end(); ++it) { + YamlPreprocessor::ProcessNodes(it->second); + } + break; + case YAML::NodeType::Scalar: + if (node.as().compare(0, 5, "$eval") == 0) { + ProcessScalarNode(node); + } + break; + default: + ROS_DEBUG_STREAM( + "Yaml Preprocessor found an unexpected type: " << node.Type()); + break; + } +} + +void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { + std::string value = node.as().substr(5); // omit the $parse + boost::algorithm::trim(value); // trim whitespace + ROS_INFO_STREAM("Attempting to parse lua " << value); + + if (value.find("return ") == std::string::npos) { // Has no return statement + value = "return " + value; + } + + // Create the Lua context + lua_State *L = luaL_newstate(); + luaL_openlibs(L); + lua_pushcfunction(L, YamlPreprocessor::LuaGetEnv); + lua_setglobal(L, "env"); + lua_pushcfunction(L, YamlPreprocessor::LuaGetParam); + lua_setglobal(L, "param"); + + try { /* Attempt to run the Lua string and parse its results */ + int error = luaL_dostring(L, value.c_str()); + if (error) { + ROS_ERROR_STREAM(lua_tostring(L, -1)); + lua_pop(L, 1); /* pop error message from the stack */ + } else { + int t = lua_type(L, 1); + if (t == LUA_TNIL) { + node = ""; + ROS_INFO_STREAM("Preprocessor parsed " << value << " as empty string"); + } else if (t == LUA_TBOOLEAN) { + ROS_INFO_STREAM("Preprocessor parsed " + << value << " as bool " + << (lua_toboolean(L, 1) ? "true" : "false")); + node = lua_toboolean(L, 1) ? "true" : "false"; + } else if (t == LUA_TSTRING || t == LUA_TNUMBER) { + ROS_INFO_STREAM("Preprocessor parsed " << value << " as " + << lua_tostring(L, 1)); + node = lua_tostring(L, 1); + } else { + ROS_ERROR_STREAM("No lua output for " << value); + } + } + } catch ( + ...) { /* Something went wrong parsing the lua, or gettings its results */ + ROS_ERROR_STREAM("Lua error in: " << value); + } +} + +YAML::Node YamlPreprocessor::LoadParse(const std::string &path) { + YAML::Node node; + + try { + node = YAML::LoadFile(path); + } catch (const YAML::BadFile &e) { + throw YAMLException("File does not exist, path=" + path); + } catch (const YAML::ParserException &e) { + throw YAMLException("Malformatted file, path=" + path, e); + } catch (const YAML::Exception &e) { + throw YAMLException("Error loading file, path=" + path, e); + } + + YamlPreprocessor::Parse(node); + return node; +} + +int YamlPreprocessor::LuaGetEnv(lua_State *L) { + const char *name = lua_tostring(L, 1); + const char *env = std::getenv(name); + + if (lua_gettop(L) == 2 && env == NULL) { // use default + if (lua_isnumber(L, 2)) { + lua_pushnumber(L, lua_tonumber(L, 2)); + } else if (lua_isstring(L, 2)) { + lua_pushstring(L, lua_tostring(L, 2)); + } else if (lua_isboolean(L, 2)) { + lua_pushboolean(L, lua_toboolean(L, 2)); + } + } else { // no default + if (env == NULL) { // Push back a nil + ROS_WARN_STREAM("No environment variable for: " << name); + lua_pushnil(L); + } else { + ROS_WARN_STREAM("Found env for " << name); + try { // Try to push a number + double x = boost::lexical_cast(env); + lua_pushnumber(L, x); + } catch (boost::bad_lexical_cast &) { // Otherwise it's a string + lua_pushstring(L, env); + } + } + } + + return 1; // 1 return value +} + +int YamlPreprocessor::LuaGetParam(lua_State *L) { + const char *name = lua_tostring(L, 1); + std::string param_s; + double param_d; + bool param_b; + + if (lua_gettop(L) == 2 && !ros::param::has(name)) { // use default + if (lua_isnumber(L, 2)) { + lua_pushnumber(L, lua_tonumber(L, 2)); + } else if (lua_isboolean(L, 2)) { + lua_pushboolean(L, lua_toboolean(L, 2)); + } else if (lua_isstring(L, 2)) { + lua_pushstring(L, lua_tostring(L, 2)); + } else { + ROS_WARN_STREAM("Couldn't load int/double/string value at param " + << name); + lua_pushnil(L); + } + } else { // no default + if (!ros::param::has(name)) { // Push back a nil + ROS_WARN_STREAM("No rosparam found for: " << name); + lua_pushnil(L); + } else { + if (ros::param::get(name, param_d)) { + lua_pushnumber(L, param_d); + } else if (ros::param::get(name, param_s)) { + lua_pushstring(L, param_s.c_str()); + } else if (ros::param::get(name, param_b)) { + lua_pushstring(L, param_b ? "true" : "false"); + } else { + ROS_WARN_STREAM("Couldn't load int/double/string value at param " + << name); + lua_pushnil(L); + } + } + } + + return 1; // 1 return value +} +} diff --git a/flatland_server/src/yaml_reader.cpp b/flatland_server/src/yaml_reader.cpp index 6c3df6f1..50a31a27 100644 --- a/flatland_server/src/yaml_reader.cpp +++ b/flatland_server/src/yaml_reader.cpp @@ -1,253 +1,253 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name yaml_reader.h - * @brief Defines yaml_reader - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -namespace flatland_server { - -YamlReader::YamlReader() : node_(YAML::Node()) { - SetErrorInfo("_NONE_", "_NONE_"); -} - -YamlReader::YamlReader(const YAML::Node &node) : node_(node) { - YamlPreprocessor::Parse(node_); - SetErrorInfo("_NONE_", "_NONE_"); -} - -YamlReader::YamlReader(const std::string &path) { - try { - node_ = YAML::LoadFile(path); - YamlPreprocessor::Parse(node_); - } catch (const YAML::BadFile &e) { - throw YAMLException("File does not exist, path=" + Q(path)); - - } catch (const YAML::ParserException &e) { - throw YAMLException("Malformatted file, path=" + Q(path), e); - - } catch (const YAML::Exception &e) { - throw YAMLException("Error loading file, path=" + Q(path), e); - } - - SetErrorInfo("_NONE_", "_NONE_"); - SetFile(path); -} - -YAML::Node YamlReader::Node() { return node_; } - -bool YamlReader::IsNodeNull() { return node_.IsNull(); } - -int YamlReader::NodeSize() { return node_.size(); } - -YamlReader YamlReader::Subnode(int index, NodeTypeCheck type_check, - std::string subnode_location) { - YamlReader reader(node_[index]); - - // default to use the error location message of its parent - std::string location = subnode_location == "" - ? (entry_location_ + " " + entry_name_) - : subnode_location; - reader.SetErrorInfo(location, "index=" + std::to_string(index)); - reader.SetFile(file_path_); - - if (reader.IsNodeNull()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " does not exist" + reader.fmt_in_); - } else if (type_check == NodeTypeCheck::MAP && !node_[index].IsMap()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " must be a map" + reader.fmt_in_); - - } else if (type_check == NodeTypeCheck::LIST && !node_[index].IsSequence()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " must be a list" + reader.fmt_in_); - } - - return reader; -} - -YamlReader YamlReader::Subnode(const std::string &key, NodeTypeCheck type_check, - std::string subnode_location) { - accessed_keys_.insert(key); - YamlReader reader(node_[key]); - - // default to use the error location message of its parent - std::string location = subnode_location == "" - ? (entry_location_ + " " + entry_name_) - : subnode_location; - reader.SetErrorInfo(location, Q(key)); - reader.SetFile(file_path_); - - if (!node_[key]) { - throw YAMLException("Entry " + Q(key) + " does not exist" + reader.fmt_in_); - } else if (type_check == NodeTypeCheck::MAP && !node_[key].IsMap()) { - throw YAMLException("Entry " + Q(key) + " must be a map" + reader.fmt_in_); - } else if (type_check == NodeTypeCheck::LIST && !node_[key].IsSequence()) { - throw YAMLException("Entry " + Q(key) + " must be a list" + reader.fmt_in_); - } - - return reader; -} - -YamlReader YamlReader::SubnodeOpt(const std::string &key, - NodeTypeCheck type_check, - std::string subnode_location) { - if (!node_[key]) { - accessed_keys_.insert(key); - return YamlReader(YAML::Node()); - } - return Subnode(key, type_check, subnode_location); -} - -Vec2 YamlReader::GetVec2(const std::string &key) { - std::array v = GetArray(key); - return Vec2(v[0], v[1]); -} - -Vec2 YamlReader::GetVec2(const std::string &key, const Vec2 &vec2) { - if (!node_[key]) { - accessed_keys_.insert(key); - return vec2; - } - - return GetVec2(key); -} - -Color YamlReader::GetColor(const std::string &key, const Color &default_val) { - std::array v = GetArray( - key, {default_val.r, default_val.g, default_val.b, default_val.a}); - return Color(v[0], v[1], v[2], v[3]); -} - -Pose YamlReader::GetPose(const std::string &key) { - std::array v = GetArray(key); - return Pose(v[0], v[1], v[2]); -} - -Pose YamlReader::GetPose(const std::string &key, const Pose &default_val) { - if (!node_[key]) { - accessed_keys_.insert(key); - return default_val; - } - - return GetPose(key); -} - -void YamlReader::SetErrorInfo(std::string entry_location, - std::string entry_name) { - boost::algorithm::trim(entry_location); - boost::algorithm::trim(entry_name); - - if (entry_location == "_NONE_") { - entry_location_ = ""; - } else if (entry_location != "") { - entry_location_ = entry_location; - } - - if (entry_name == "_NONE_") { - entry_name_ = ""; - } else if (entry_name != "") { - entry_name_ = entry_name; - } - - if (entry_location_.size() == 0) { - fmt_in_ = ""; - } else { - std::string msg = entry_location_; - boost::algorithm::to_lower(msg); - fmt_in_ = " (in " + msg + ")"; - } - - if (entry_name_.size() == 0) { - fmt_name_ = ""; - } else { - std::string msg = entry_name_; - boost::algorithm::to_lower(msg); - fmt_name_ = " " + msg; - } -} - -void YamlReader::SetFile(const std::string &file_path) { - if (file_path == "_NONE_") { - file_path_ = ""; - } else { - file_path_ = file_path; - } - - filename_ = boost::filesystem::path(file_path).filename().string(); -} - -void YamlReader::EnsureAccessedAllKeys() { - if (!node_.IsMap()) { - throw YAMLException("Entry" + fmt_name_ + " should be a map" + fmt_in_); - } - - std::vector unused_keys; - std::vector keys; - - for (const auto &k : node_) { - keys.push_back(k.first.as()); - } - - std::sort(keys.begin(), keys.end()); - - for (const auto &key : keys) { - if (accessed_keys_.count(key) == 0) { - unused_keys.push_back("\"" + key + "\""); - } - } - - std::string keys_str = "{" + boost::algorithm::join(keys, ", ") + "}"; - std::string unused_keys_str = - "{" + boost::algorithm::join(unused_keys, ", ") + "}"; - - if (unused_keys.size() > 0) { - throw YAMLException("Entry" + fmt_name_ + - " contains unrecognized entry(s) " + unused_keys_str + - fmt_in_); - } -} -} \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name yaml_reader.h + * @brief Defines yaml_reader + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +namespace flatland_server { + +YamlReader::YamlReader() : node_(YAML::Node()) { + SetErrorInfo("_NONE_", "_NONE_"); +} + +YamlReader::YamlReader(const YAML::Node &node) : node_(node) { + YamlPreprocessor::Parse(node_); + SetErrorInfo("_NONE_", "_NONE_"); +} + +YamlReader::YamlReader(const std::string &path) { + try { + node_ = YAML::LoadFile(path); + YamlPreprocessor::Parse(node_); + } catch (const YAML::BadFile &e) { + throw YAMLException("File does not exist, path=" + Q(path)); + + } catch (const YAML::ParserException &e) { + throw YAMLException("Malformatted file, path=" + Q(path), e); + + } catch (const YAML::Exception &e) { + throw YAMLException("Error loading file, path=" + Q(path), e); + } + + SetErrorInfo("_NONE_", "_NONE_"); + SetFile(path); +} + +YAML::Node YamlReader::Node() { return node_; } + +bool YamlReader::IsNodeNull() { return node_.IsNull(); } + +int YamlReader::NodeSize() { return node_.size(); } + +YamlReader YamlReader::Subnode(int index, NodeTypeCheck type_check, + std::string subnode_location) { + YamlReader reader(node_[index]); + + // default to use the error location message of its parent + std::string location = subnode_location == "" + ? (entry_location_ + " " + entry_name_) + : subnode_location; + reader.SetErrorInfo(location, "index=" + std::to_string(index)); + reader.SetFile(file_path_); + + if (reader.IsNodeNull()) { + throw YAMLException("Entry index=" + std::to_string(index) + + " does not exist" + reader.fmt_in_); + } else if (type_check == NodeTypeCheck::MAP && !node_[index].IsMap()) { + throw YAMLException("Entry index=" + std::to_string(index) + + " must be a map" + reader.fmt_in_); + + } else if (type_check == NodeTypeCheck::LIST && !node_[index].IsSequence()) { + throw YAMLException("Entry index=" + std::to_string(index) + + " must be a list" + reader.fmt_in_); + } + + return reader; +} + +YamlReader YamlReader::Subnode(const std::string &key, NodeTypeCheck type_check, + std::string subnode_location) { + accessed_keys_.insert(key); + YamlReader reader(node_[key]); + + // default to use the error location message of its parent + std::string location = subnode_location == "" + ? (entry_location_ + " " + entry_name_) + : subnode_location; + reader.SetErrorInfo(location, Q(key)); + reader.SetFile(file_path_); + + if (!node_[key]) { + throw YAMLException("Entry " + Q(key) + " does not exist" + reader.fmt_in_); + } else if (type_check == NodeTypeCheck::MAP && !node_[key].IsMap()) { + throw YAMLException("Entry " + Q(key) + " must be a map" + reader.fmt_in_); + } else if (type_check == NodeTypeCheck::LIST && !node_[key].IsSequence()) { + throw YAMLException("Entry " + Q(key) + " must be a list" + reader.fmt_in_); + } + + return reader; +} + +YamlReader YamlReader::SubnodeOpt(const std::string &key, + NodeTypeCheck type_check, + std::string subnode_location) { + if (!node_[key]) { + accessed_keys_.insert(key); + return YamlReader(YAML::Node()); + } + return Subnode(key, type_check, subnode_location); +} + +Vec2 YamlReader::GetVec2(const std::string &key) { + std::array v = GetArray(key); + return Vec2(v[0], v[1]); +} + +Vec2 YamlReader::GetVec2(const std::string &key, const Vec2 &vec2) { + if (!node_[key]) { + accessed_keys_.insert(key); + return vec2; + } + + return GetVec2(key); +} + +Color YamlReader::GetColor(const std::string &key, const Color &default_val) { + std::array v = GetArray( + key, {default_val.r, default_val.g, default_val.b, default_val.a}); + return Color(v[0], v[1], v[2], v[3]); +} + +Pose YamlReader::GetPose(const std::string &key) { + std::array v = GetArray(key); + return Pose(v[0], v[1], v[2]); +} + +Pose YamlReader::GetPose(const std::string &key, const Pose &default_val) { + if (!node_[key]) { + accessed_keys_.insert(key); + return default_val; + } + + return GetPose(key); +} + +void YamlReader::SetErrorInfo(std::string entry_location, + std::string entry_name) { + boost::algorithm::trim(entry_location); + boost::algorithm::trim(entry_name); + + if (entry_location == "_NONE_") { + entry_location_ = ""; + } else if (entry_location != "") { + entry_location_ = entry_location; + } + + if (entry_name == "_NONE_") { + entry_name_ = ""; + } else if (entry_name != "") { + entry_name_ = entry_name; + } + + if (entry_location_.size() == 0) { + fmt_in_ = ""; + } else { + std::string msg = entry_location_; + boost::algorithm::to_lower(msg); + fmt_in_ = " (in " + msg + ")"; + } + + if (entry_name_.size() == 0) { + fmt_name_ = ""; + } else { + std::string msg = entry_name_; + boost::algorithm::to_lower(msg); + fmt_name_ = " " + msg; + } +} + +void YamlReader::SetFile(const std::string &file_path) { + if (file_path == "_NONE_") { + file_path_ = ""; + } else { + file_path_ = file_path; + } + + filename_ = boost::filesystem::path(file_path).filename().string(); +} + +void YamlReader::EnsureAccessedAllKeys() { + if (!node_.IsMap()) { + throw YAMLException("Entry" + fmt_name_ + " should be a map" + fmt_in_); + } + + std::vector unused_keys; + std::vector keys; + + for (const auto &k : node_) { + keys.push_back(k.first.as()); + } + + std::sort(keys.begin(), keys.end()); + + for (const auto &key : keys) { + if (accessed_keys_.count(key) == 0) { + unused_keys.push_back("\"" + key + "\""); + } + } + + std::string keys_str = "{" + boost::algorithm::join(keys, ", ") + "}"; + std::string unused_keys_str = + "{" + boost::algorithm::join(unused_keys, ", ") + "}"; + + if (unused_keys.size() > 0) { + throw YAMLException("Entry" + fmt_name_ + + " contains unrecognized entry(s) " + unused_keys_str + + fmt_in_); + } +} +} diff --git a/flatland_server/test/benchmark_world/cleaner.model.yaml b/flatland_server/test/benchmark_world/cleaner.model.yaml new file mode 100644 index 00000000..a6d17f06 --- /dev/null +++ b/flatland_server/test/benchmark_world/cleaner.model.yaml @@ -0,0 +1,94 @@ + +bodies: + - name: base + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 100 + points: [ [-1.03, -0.337], + [.07983, -0.337], + [.30, -.16111], + [.30, .16111], + [.07983, 0.337], + [-1.03, 0.337] ] + - name: front_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0250], + [ 0.0875, 0.0250], + [-0.0875, 0.0250], + [-0.0875, -0.0250]] + + - name: rear_left_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + + - name: rear_right_wheel + color: [1, 1, 1, 0.75] + footprints: + - type: polygon + density: 1.0 + points: [[ 0.0875, -0.0255], + [ 0.0875, 0.0255], + [-0.0875, 0.0255], + [-0.0875, -0.0255]] + +joints: + - type: revolute + name: front_wheel_revolute + bodies: + - name: front_wheel + anchor: [0, 0] + - name: base + anchor: [0, 0] + + - type: weld + name: rear_right_wheel_weld + bodies: + - name: rear_left_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, 0.29] + + - type: weld + name: rear_left_wheel_weld + bodies: + - name: rear_right_wheel + anchor: [0, 0] + - name: base + anchor: [-0.83, -0.29] + +plugins: + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: TricycleDrive + name: cleaner_drive + body: base + front_wheel_joint: front_wheel_revolute + rear_left_wheel_joint: rear_left_wheel_weld + rear_right_wheel_joint: rear_right_wheel_weld + odom_frame_id: map + + - type: Laser + name: laser_front + frame: laser_front + topic: scan + body: base + broadcast_tf: true + origin: [0.28, 0, 0] + range: 20 + always_publish: true + angle: {min: -2.356194490192345, max: 2.356194490192345, increment: 0.004363323129985824} + noise_std_dev: 0.05 + update_rate: 40 diff --git a/flatland_server/test/benchmark_world/map.png b/flatland_server/test/benchmark_world/map.png new file mode 100644 index 00000000..a29b0ba1 Binary files /dev/null and b/flatland_server/test/benchmark_world/map.png differ diff --git a/flatland_server/test/benchmark_world/map.yaml b/flatland_server/test/benchmark_world/map.yaml new file mode 100644 index 00000000..ac269fbf --- /dev/null +++ b/flatland_server/test/benchmark_world/map.yaml @@ -0,0 +1,7 @@ +image: map.png +resolution: 0.050000 +origin: [-16.600000, -6.650000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/flatland_server/test/benchmark_world/map3d.png b/flatland_server/test/benchmark_world/map3d.png new file mode 100644 index 00000000..acc07667 Binary files /dev/null and b/flatland_server/test/benchmark_world/map3d.png differ diff --git a/flatland_server/test/benchmark_world/map3d.yaml b/flatland_server/test/benchmark_world/map3d.yaml new file mode 100644 index 00000000..6cec7384 --- /dev/null +++ b/flatland_server/test/benchmark_world/map3d.yaml @@ -0,0 +1,6 @@ +image: map3d.png +resolution: 0.050000 +origin: [-16.600000, -6.650000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 \ No newline at end of file diff --git a/flatland_server/test/benchmark_world/turtlebot.model.yaml b/flatland_server/test/benchmark_world/turtlebot.model.yaml new file mode 100644 index 00000000..60be6a62 --- /dev/null +++ b/flatland_server/test/benchmark_world/turtlebot.model.yaml @@ -0,0 +1,53 @@ +# Turtlebot w/ lidar +bodies: # List of named bodies + - name: base + pose: [0, 0, 0] + type: dynamic + color: [1, 1, 1, 0.75] + footprints: + - type: circle + radius: 0.5 + center: [-1, 0.0] + density: 1 + + - type: polygon + sensor: true + points: [[-.45, -.05], [-.45, 0.05], [-.35, 0.05], [-.35, -0.05]] + layers: [] + density: 1 + + - type: polygon + sensor: true + points: [[-.125, -.4], [-.125, -.3], [.125, -.3], [.125, -.4]] + density: 1 + + - type: polygon + sensor: true + points: [[-.125, .4], [-.125, .3], [.125, .3], [.125, .4]] + density: 100000 +plugins: + - type: DiffDrive + name: turtlebot_drive + body: base + odom_frame_id: map + pub_rate: 10 + # odom_pose_noise: [0.1, 0.1, 0.1] + # odom_twist_noise: [0.1, 0.1, 0.1] + # odom_pose_covariance: [0.00, 0.01, 0.02, 0.03, 0.04, 0.05, + # 0.06, 0.07, 0.08, 0.09, 0.10, 0.11, + # 0.12, 0.13, 0.14, 0.15, 0.16, 0.17, + # 0.18, 0.19, 0.20, 0.21, 0.22, 0.23, + # 0.24, 0.25, 0.26, 0.27, 0.28, 0.29, + # 0.30, 0.31, 0.32, 0.33, 0.34, 0.35] + + - type: ModelTfPublisher + name: tf_publisher + publish_tf_world: true + + - type: Laser + name: laser_front + body: base + always_publish: true + range: 20 + update_rate: 25 # Hz + angle: {min: -2.35619, max: 2.35619, increment: 0.004363323} # 1080 scans, 270 deg arc, 0.25deg inc diff --git a/flatland_server/test/benchmark_world/world.yaml b/flatland_server/test/benchmark_world/world.yaml new file mode 100644 index 00000000..887e29ad --- /dev/null +++ b/flatland_server/test/benchmark_world/world.yaml @@ -0,0 +1,16 @@ +properties: {} # For later use +layers: # Support for arbitrary number of layers + - name: "2d" # layer 0 named "2d" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + - name: "3d" # layer 1 named "3d" + map: "map3d.yaml" + color: [1.0, 0.0, 0.0, 0.5] # red 50% transparent +models: + - name: cleaner1 + pose: [12, 0, 0] + model: "cleaner.model.yaml" + # - name: turtlebot1 + # pose: [12, 0, 0] + # model: "turtlebot.model.yaml" + \ No newline at end of file diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index 31c19e17..1182747c 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -1,138 +1,138 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name collision_filter_registry_test.cpp - * @brief Testing collision filter registry - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -using namespace flatland_server; - -typedef CollisionFilterRegistry CFR; - -class CollisionFilterRegistryTest : public ::testing::Test { - protected: - CollisionFilterRegistry cfr; -}; - -TEST_F(CollisionFilterRegistryTest, empty_test) { - EXPECT_FALSE(cfr.IsLayersFull()); - EXPECT_EQ(cfr.LookUpLayerId("random_layer"), CFR::LAYER_NOT_EXIST); - - std::vector layer_names = cfr.GetAllLayers(); - EXPECT_EQ(layer_names.size(), 0); -} - -TEST_F(CollisionFilterRegistryTest, register_collide_test) { - EXPECT_EQ(cfr.RegisterCollide(), 1); - EXPECT_EQ(cfr.RegisterCollide(), 2); - EXPECT_EQ(cfr.RegisterCollide(), 3); - EXPECT_EQ(cfr.RegisterCollide(), 4); - EXPECT_EQ(cfr.RegisterCollide(), 5); -} - -TEST_F(CollisionFilterRegistryTest, register_no_collide_test) { - EXPECT_EQ(cfr.RegisterNoCollide(), -1); - EXPECT_EQ(cfr.RegisterNoCollide(), -2); - EXPECT_EQ(cfr.RegisterNoCollide(), -3); - EXPECT_EQ(cfr.RegisterNoCollide(), -4); - EXPECT_EQ(cfr.RegisterNoCollide(), -5); -} - -TEST_F(CollisionFilterRegistryTest, register_layers_test) { - EXPECT_EQ(CFR::MAX_LAYERS, 16); - - EXPECT_EQ(cfr.RegisterLayer("layer1"), 0); - EXPECT_EQ(cfr.RegisterLayer("layer2"), 1); - EXPECT_EQ(cfr.RegisterLayer("layer3"), 2); - EXPECT_EQ(cfr.RegisterLayer("layer4"), 3); - EXPECT_EQ(cfr.RegisterLayer("layer5"), 4); - - EXPECT_EQ(cfr.LookUpLayerId("layer1"), 0); - EXPECT_EQ(cfr.LookUpLayerId("layer2"), 1); - EXPECT_EQ(cfr.LookUpLayerId("layer3"), 2); - EXPECT_EQ(cfr.LookUpLayerId("layer4"), 3); - EXPECT_EQ(cfr.LookUpLayerId("layer5"), 4); - - std::vector layer_names = cfr.GetAllLayers(); - EXPECT_EQ(layer_names.size(), 5); - - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer1") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer2") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer3") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer4") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer5") != - layer_names.end()); - - EXPECT_EQ(cfr.RegisterLayer("layer5"), CFR::LAYER_ALREADY_EXIST); - EXPECT_EQ(cfr.RegisterLayer("layer6"), 5); - EXPECT_EQ(cfr.RegisterLayer("layer7"), 6); - EXPECT_EQ(cfr.RegisterLayer("layer8"), 7); - EXPECT_EQ(cfr.RegisterLayer("layer9"), 8); - EXPECT_EQ(cfr.RegisterLayer("layer10"), 9); - EXPECT_EQ(cfr.RegisterLayer("layer11"), 10); - EXPECT_EQ(cfr.RegisterLayer("layer12"), 11); - EXPECT_EQ(cfr.RegisterLayer("layer13"), 12); - EXPECT_EQ(cfr.RegisterLayer("layer14"), 13); - EXPECT_EQ(cfr.RegisterLayer("layer15"), 14); - EXPECT_EQ(cfr.RegisterLayer("layer16"), 15); - EXPECT_EQ(cfr.RegisterLayer("layer17"), CFR::LAYERS_FULL); - - EXPECT_TRUE(cfr.IsLayersFull()); - layer_names = cfr.GetAllLayers(); - - EXPECT_EQ(layer_names.size(), 16); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name collision_filter_registry_test.cpp + * @brief Testing collision filter registry + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +using namespace flatland_server; + +typedef CollisionFilterRegistry CFR; + +class CollisionFilterRegistryTest : public ::testing::Test { + protected: + CollisionFilterRegistry cfr; +}; + +TEST_F(CollisionFilterRegistryTest, empty_test) { + EXPECT_FALSE(cfr.IsLayersFull()); + EXPECT_EQ(cfr.LookUpLayerId("random_layer"), CFR::LAYER_NOT_EXIST); + + std::vector layer_names = cfr.GetAllLayers(); + EXPECT_EQ(layer_names.size(), 0); +} + +TEST_F(CollisionFilterRegistryTest, register_collide_test) { + EXPECT_EQ(cfr.RegisterCollide(), 1); + EXPECT_EQ(cfr.RegisterCollide(), 2); + EXPECT_EQ(cfr.RegisterCollide(), 3); + EXPECT_EQ(cfr.RegisterCollide(), 4); + EXPECT_EQ(cfr.RegisterCollide(), 5); +} + +TEST_F(CollisionFilterRegistryTest, register_no_collide_test) { + EXPECT_EQ(cfr.RegisterNoCollide(), -1); + EXPECT_EQ(cfr.RegisterNoCollide(), -2); + EXPECT_EQ(cfr.RegisterNoCollide(), -3); + EXPECT_EQ(cfr.RegisterNoCollide(), -4); + EXPECT_EQ(cfr.RegisterNoCollide(), -5); +} + +TEST_F(CollisionFilterRegistryTest, register_layers_test) { + EXPECT_EQ(CFR::MAX_LAYERS, 16); + + EXPECT_EQ(cfr.RegisterLayer("layer1"), 0); + EXPECT_EQ(cfr.RegisterLayer("layer2"), 1); + EXPECT_EQ(cfr.RegisterLayer("layer3"), 2); + EXPECT_EQ(cfr.RegisterLayer("layer4"), 3); + EXPECT_EQ(cfr.RegisterLayer("layer5"), 4); + + EXPECT_EQ(cfr.LookUpLayerId("layer1"), 0); + EXPECT_EQ(cfr.LookUpLayerId("layer2"), 1); + EXPECT_EQ(cfr.LookUpLayerId("layer3"), 2); + EXPECT_EQ(cfr.LookUpLayerId("layer4"), 3); + EXPECT_EQ(cfr.LookUpLayerId("layer5"), 4); + + std::vector layer_names = cfr.GetAllLayers(); + EXPECT_EQ(layer_names.size(), 5); + + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer1") != + layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer2") != + layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer3") != + layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer4") != + layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer5") != + layer_names.end()); + + EXPECT_EQ(cfr.RegisterLayer("layer5"), CFR::LAYER_ALREADY_EXIST); + EXPECT_EQ(cfr.RegisterLayer("layer6"), 5); + EXPECT_EQ(cfr.RegisterLayer("layer7"), 6); + EXPECT_EQ(cfr.RegisterLayer("layer8"), 7); + EXPECT_EQ(cfr.RegisterLayer("layer9"), 8); + EXPECT_EQ(cfr.RegisterLayer("layer10"), 9); + EXPECT_EQ(cfr.RegisterLayer("layer11"), 10); + EXPECT_EQ(cfr.RegisterLayer("layer12"), 11); + EXPECT_EQ(cfr.RegisterLayer("layer13"), 12); + EXPECT_EQ(cfr.RegisterLayer("layer14"), 13); + EXPECT_EQ(cfr.RegisterLayer("layer15"), 14); + EXPECT_EQ(cfr.RegisterLayer("layer16"), 15); + EXPECT_EQ(cfr.RegisterLayer("layer17"), CFR::LAYERS_FULL); + + EXPECT_TRUE(cfr.IsLayersFull()); + layer_names = cfr.GetAllLayers(); + + EXPECT_EQ(layer_names.size(), 16); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/conestogo_office_test/world.yaml b/flatland_server/test/conestogo_office_test/world.yaml index ad143c78..a0d1d544 100644 --- a/flatland_server/test/conestogo_office_test/world.yaml +++ b/flatland_server/test/conestogo_office_test/world.yaml @@ -16,14 +16,14 @@ models: - name: sensor pose: [2, 0, 0] model: "sensor.model.yaml" -plugins: - - name: test1 - type: RandomWall - layer: "2d" - num_of_walls: 10 - wall_wall_dist: 1 - double_wall: true - robot_name: cleaner1 +# plugins: +# - name: test1 +# type: RandomWall +# layer: "2d" +# num_of_walls: 10 +# wall_wall_dist: 1 +# double_wall: true +# robot_name: cleaner1 # - name: turtlebot1 diff --git a/flatland_server/test/conestogo_office_test/world_plugins.yaml b/flatland_server/test/conestogo_office_test/world_plugins.yaml new file mode 100644 index 00000000..fdaee9ad --- /dev/null +++ b/flatland_server/test/conestogo_office_test/world_plugins.yaml @@ -0,0 +1,9 @@ +properties: {} +plugins: + - name: test1 + type: RandomWall + layer: "2d" + num_of_walls: 10 + wall_wall_dist: 1 + double_wall: true + robot_name: cleaner1 diff --git a/flatland_server/test/debug_visualization_test.cpp b/flatland_server/test/debug_visualization_test.cpp index 74b81cd9..a0fbd4f8 100644 --- a/flatland_server/test/debug_visualization_test.cpp +++ b/flatland_server/test/debug_visualization_test.cpp @@ -1,510 +1,437 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name null.cpp - * @brief Sanity check / example test file - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/debug_visualization.h" -#include -#include -#include -#include -#include -#include - -// Test the bodyToMarkers method on a polygon shape -TEST(DebugVizTest, testBodyToMarkersPolygon) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - bodyDef.type = b2_dynamicBody; - bodyDef.position.Set(3.0f, 4.0f); - bodyDef.angle = M_PI_2; - b2Body* body = world.CreateBody(&bodyDef); - - b2PolygonShape dynamicBox; - dynamicBox.SetAsBox(1.0f, 2.0f); - b2FixtureDef fixtureDef; - fixtureDef.shape = &dynamicBox; - fixtureDef.density = 1.0f; - fixtureDef.friction = 0.3f; - body->CreateFixture(&fixtureDef); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.5, 0.7); - // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); - - // Check that - ASSERT_EQ(markers.markers[0].header.frame_id, "map"); - ASSERT_EQ(markers.markers[0].header.stamp.sec, 0); - ASSERT_EQ(markers.markers[0].header.stamp.nsec, 0); - - // Check color setting - ASSERT_NEAR(markers.markers[0].color.r, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].color.g, 0.0, 1e-5); - ASSERT_NEAR(markers.markers[0].color.b, 0.5, 1e-5); - ASSERT_NEAR(markers.markers[0].color.a, 0.7, 1e-5); - - // Check position - ASSERT_NEAR(markers.markers[0].pose.position.x, 3.0, 1e-5); - ASSERT_NEAR(markers.markers[0].pose.position.y, 4.0, 1e-5); - ASSERT_NEAR(markers.markers[0].pose.position.z, 0.0, 1e-5); - - // Check orientation - ASSERT_NEAR(markers.markers[0].pose.orientation.x, 0.0, 1e-5); - ASSERT_NEAR(markers.markers[0].pose.orientation.y, 0.0, 1e-5); - ASSERT_NEAR(markers.markers[0].pose.orientation.z, 0.70710678118, 1e-5); - ASSERT_NEAR(markers.markers[0].pose.orientation.w, 0.70710678118, 1e-5); - - // Check the marker shape - ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_STRIP); - ASSERT_EQ(markers.markers[0].points.size(), - 5); // box as line strip: 0, 1, 2, 3, 0 - ASSERT_NEAR(markers.markers[0].points[0].x, -1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[0].y, -2.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].y, -2.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[2].x, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[2].y, 2.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].x, -1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].y, 2.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[4].x, -1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[4].y, -2.0, 1e-5); -} - -// Test the bodyToMarkers method on a circle shape -TEST(DebugVizTest, testBodyToMarkersCircle) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - bodyDef.type = b2_dynamicBody; - bodyDef.position.Set(3.0f, 4.0f); - bodyDef.angle = M_PI_2; - b2Body* body = world.CreateBody(&bodyDef); - - b2FixtureDef fixtureDef; - b2CircleShape circle; - circle.m_p.Set(2.0f, 3.0f); - circle.m_radius = 0.2f; - fixtureDef.shape = &circle; - body->CreateFixture(&fixtureDef); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); - - // Check the marker shape - ASSERT_EQ(markers.markers[0].type, markers.markers[0].SPHERE_LIST); - ASSERT_NEAR(markers.markers[0].scale.x, 0.4, 1e-5); - ASSERT_NEAR(markers.markers[0].scale.y, 0.4, 1e-5); - ASSERT_NEAR(markers.markers[0].scale.z, 0.01, 1e-5); -} - -// Test the bodyToMarkers method on a edge shape -TEST(DebugVizTest, testBodyToMarkersEdge) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - - b2FixtureDef fixtureDef; - b2EdgeShape edge; - edge.m_vertex1.Set(0.5, 1.5); - edge.m_vertex2.Set(3.5, 2.0); - fixtureDef.shape = &edge; - body->CreateFixture(&fixtureDef); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); - - // Check the marker shape - ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 2); - ASSERT_NEAR(markers.markers[0].points[0].x, 0.5, 1e-5); - ASSERT_NEAR(markers.markers[0].points[0].y, 1.5, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].x, 3.5, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].y, 2.0, 1e-5); -} - -// Test the bodyToMarkers method on an unsupported shape -TEST(DebugVizTest, testBodyToMarkersUnsupported) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - - b2FixtureDef fixtureDef; - b2Vec2 vs[4]; - vs[0].Set(1.7f, 0.0f); - vs[1].Set(1.0f, 0.25f); - vs[2].Set(0.0f, 0.0f); - vs[3].Set(-1.7f, 0.4f); - b2ChainShape chain; - chain.CreateChain(vs, 4); - fixtureDef.shape = &chain; - body->CreateFixture(&fixtureDef); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - // check that marker was not created - ASSERT_EQ(markers.markers.size(), 0); -} - -// test bodyToMarkers with a body with multiple fixtures -TEST(DebugVizTest, testBodyToMarkersMultifixture) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - - // body 2 before body 1 because fixture ordering is LIFO in box2d - b2FixtureDef fixtureDef, fixtureDef2; - b2EdgeShape edge, edge2; - - edge2.m_vertex1.Set(-1.0, 3.0); - edge2.m_vertex2.Set(5.0, 7.0); - fixtureDef2.shape = &edge2; - body->CreateFixture(&fixtureDef2); - - edge.m_vertex1.Set(0.0, 1.0); - edge.m_vertex2.Set(1.0, 2.0); - fixtureDef.shape = &edge; - body->CreateFixture(&fixtureDef); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - // check that one marker was created - ASSERT_EQ(markers.markers.size(), 1); - - // Check the 1st marker - ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 4); - ASSERT_NEAR(markers.markers[0].points[0].x, 0.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[0].y, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].y, 2.0, 1e-5); - - // check the "2nd marker" - ASSERT_NEAR(markers.markers[0].points[2].x, -1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[2].y, 3.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].x, 5.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].y, 7.0, 1e-5); -} - -// test bodyToMarkers with multiple bodies -TEST(DebugVizTest, testBodyToMarkersMultibody) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - b2Body* body2 = world.CreateBody(&bodyDef); - - b2FixtureDef fixtureDef, fixtureDef2; - b2EdgeShape edge, edge2; - - edge.m_vertex1.Set(0.0, 1.0); - edge.m_vertex2.Set(1.0, 2.0); - fixtureDef.shape = &edge; - body->CreateFixture(&fixtureDef); - - edge2.m_vertex1.Set(-1.0, 3.0); - edge2.m_vertex2.Set(5.0, 7.0); - fixtureDef2.shape = &edge2; - body2->CreateFixture(&fixtureDef2); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body2, 1.0, - 0.0, 0.0, 1.0); - // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); - - // Check the 1st marker - ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 4); - ASSERT_NEAR(markers.markers[0].points[0].x, 0.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[0].y, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[1].y, 2.0, 1e-5); - - // check the "2nd marker" - ASSERT_NEAR(markers.markers[0].points[2].x, -1.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[2].y, 3.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].x, 5.0, 1e-5); - ASSERT_NEAR(markers.markers[0].points[3].y, 7.0, 1e-5); -} - -// test bodyToMarkers with multiple joint -TEST(DebugVizTest, testJointToMarkersMultiJoint) { - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* b1 = world.CreateBody(&bodyDef); - b2Body* b2 = world.CreateBody(&bodyDef); - - b2WeldJointDef jd1, jd2; - jd1.bodyA = b1; - jd1.bodyB = b2; - jd1.localAnchorA = b2Vec2(0, 0); - jd1.localAnchorB = b2Vec2(0, 0); - jd2.bodyA = b1; - jd2.bodyB = b2; - jd2.localAnchorA = b2Vec2(1, 2); - jd2.localAnchorB = b2Vec2(3, 4); - - b2Joint* j1 = world.CreateJoint(&jd1); - b2Joint* j2 = world.CreateJoint(&jd2); - - visualization_msgs::MarkerArray markers; - flatland_server::DebugVisualization::Get().JointToMarkers(markers, j1, 0.1, - 0.2, 0.3, 0.4); - flatland_server::DebugVisualization::Get().JointToMarkers(markers, j2, 0.5, - 0.6, 0.7, 0.8); - // check that marker was created - ASSERT_EQ(markers.markers.size(), 4); - - // Check the 1st marker - ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 6); - - for (unsigned int i = 0; i < 6; i++) { - ASSERT_FLOAT_EQ(markers.markers[0].points[i].x, 0.0) << "index: " << i; - ASSERT_FLOAT_EQ(markers.markers[0].points[i].y, 0.0) << "index: " << i; - } - - ASSERT_FLOAT_EQ(markers.markers[0].color.r, 0.1); - ASSERT_FLOAT_EQ(markers.markers[0].color.g, 0.2); - ASSERT_FLOAT_EQ(markers.markers[0].color.b, 0.3); - ASSERT_FLOAT_EQ(markers.markers[0].color.a, 0.4); - - // Check the 2nd marker - ASSERT_EQ(markers.markers[1].type, markers.markers[1].CUBE_LIST); - ASSERT_EQ(markers.markers[1].points.size(), 4); - - for (unsigned int i = 0; i < 4; i++) { - ASSERT_FLOAT_EQ(markers.markers[1].points[i].x, 0.0) << "index: " << i; - ASSERT_FLOAT_EQ(markers.markers[1].points[i].y, 0.0) << "index: " << i; - } - - // Check the 3rd marker - ASSERT_EQ(markers.markers[2].type, markers.markers[3].LINE_LIST); - ASSERT_EQ(markers.markers[2].points.size(), 6); - ASSERT_FLOAT_EQ(markers.markers[2].points[0].x, 0.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[0].y, 0.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[1].x, 1.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[1].y, 2.0); - - ASSERT_FLOAT_EQ(markers.markers[2].points[2].x, 0.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[2].y, 0.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[3].x, 3.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[3].y, 4.0); - - ASSERT_FLOAT_EQ(markers.markers[2].points[4].x, 1.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[4].y, 2.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[5].x, 3.0); - ASSERT_FLOAT_EQ(markers.markers[2].points[5].y, 4.0); - - // Check the 4th marker - ASSERT_EQ(markers.markers[3].type, markers.markers[3].CUBE_LIST); - ASSERT_EQ(markers.markers[3].points.size(), 4); - ASSERT_FLOAT_EQ(markers.markers[3].points[0].x, 1.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[0].y, 2.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[1].x, 3.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[1].y, 4.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[2].x, 0.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[2].y, 0.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[3].x, 0.0); - ASSERT_FLOAT_EQ(markers.markers[3].points[3].y, 0.0); -} - -// A helper class to accept MarkerArray message callbacks -struct MarkerArraySubscriptionHelper { - visualization_msgs::MarkerArray markers_; - int count_; - - MarkerArraySubscriptionHelper() : count_(0) {} - - /** - * @brief callback that stores the last message and total message count - * @param msg The input message pointer - */ - void callback(const visualization_msgs::MarkerArrayConstPtr& msg) { - ++count_; - ROS_INFO("GOT ONE"); - markers_ = visualization_msgs::MarkerArray(*msg); // Copy the message - } - - /** - * @brief Wait up to 2 seconds for a specific message count - * - * @param count The message count to wait for - * - * @return true if successful - */ - bool waitForMessageCount(int count) { - ros::Rate rate(10); // throttle check to 10Hz - for (unsigned int i = 0; i < 20; i++) { - ros::spinOnce(); - if (count_ >= count) return true; - rate.sleep(); - } - return false; - } -}; - -// Test the bodyToMarkers method on an unsupported shape -TEST(DebugVizTest, testPublishMarkers) { - flatland_server::Timekeeper timekeeper; - timekeeper.SetMaxStepSize(0.01); - - b2Vec2 gravity(0.0, 0.0); - b2World world(gravity); - - b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - b2Body* body2 = world.CreateBody(&bodyDef); - - b2FixtureDef fixtureDef; - b2CircleShape circle; - circle.m_p.Set(2.0f, 3.0f); - circle.m_radius = 0.2f; - fixtureDef.shape = &circle; - body->CreateFixture(&fixtureDef); - - b2WeldJointDef joint_def; - joint_def.bodyA = body; - joint_def.bodyB = body2; - joint_def.localAnchorA = b2Vec2(0, 0); - joint_def.localAnchorB = b2Vec2(0, 0); - b2Joint* joint = world.CreateJoint(&joint_def); - - // Set up helper class subscribing to rostopic - ros::NodeHandle nh; - MarkerArraySubscriptionHelper helper; - ros::Subscriber sub = - nh.subscribe("/debug_visualization_test/debug/example", 0, - &MarkerArraySubscriptionHelper::callback, &helper); - - flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); - - // Check pre publish conditions - EXPECT_EQ(flatland_server::DebugVisualization::Get().topics_.size(), 1); - ros::spinOnce(); - EXPECT_EQ(helper.count_, 0); - EXPECT_EQ(flatland_server::DebugVisualization::Get() - .topics_["example"] - .needs_publishing, - true); - - // Check that there is a publisher - EXPECT_EQ(sub.getNumPublishers(), 1); - - // Publish - flatland_server::DebugVisualization::Get().Publish(timekeeper); - - // Verify that message was published - EXPECT_TRUE(helper.waitForMessageCount(1)); - EXPECT_EQ(helper.markers_.markers.size(), 1); - - // Publish again (should have no change- nothing needs publishing) - flatland_server::DebugVisualization::Get().Publish(timekeeper); - - // Verify that message was published - EXPECT_TRUE(helper.waitForMessageCount(1)); - EXPECT_EQ(1, helper.markers_.markers.size()); - - // Publish some more markers - flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); - // inserts two markers - flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get().Publish(timekeeper); - - // Verify that message was published - EXPECT_TRUE(helper.waitForMessageCount(2)); // Published twice - EXPECT_EQ(5, helper.markers_.markers.size()); // 5 markers in latest msg - - // Reset marker list, this empties the markers array, and topics having - // empty markers are automatically deleted - flatland_server::DebugVisualization::Get().Reset("example"); - flatland_server::DebugVisualization::Get().Publish(timekeeper); - - // Verify that message was published - EXPECT_TRUE(helper.waitForMessageCount(2)); // Published two times - - // publish again with some contents, and the topic is created again - flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get().Publish(timekeeper); - - EXPECT_TRUE(helper.waitForMessageCount(3)); - EXPECT_EQ(2, helper.markers_.markers.size()); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "debug_visualization_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name null.cpp + * @brief Sanity check / example test file + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/debug_visualization.h" +#include +#include +#include +#include +#include +#include + +// Helper: create a minimal b2WorldId for unit tests. +static b2WorldId MakeTestWorld() { + b2WorldDef wd = b2DefaultWorldDef(); + wd.gravity = {0.0f, 0.0f}; + return b2CreateWorld(&wd); +} + +// Test the bodyToMarkers method on a polygon shape +TEST(DebugVizTest, testBodyToMarkersPolygon) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {3.0f, 4.0f}; + bodyDef.rotation = b2MakeRot(M_PI_2); + b2BodyId body = b2CreateBody(world, &bodyDef); + + b2Polygon box = b2MakeBox(1.0f, 2.0f); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.friction = 0.3f; + b2CreatePolygonShape(body, &shapeDef, &box); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.5, 0.7); + // check that marker was created + ASSERT_EQ(markers.markers.size(), 1); + + // Check that + ASSERT_EQ(markers.markers[0].header.frame_id, "map"); + ASSERT_EQ(markers.markers[0].header.stamp.sec, 0); + ASSERT_EQ(markers.markers[0].header.stamp.nsec, 0); + + // Check color setting + ASSERT_NEAR(markers.markers[0].color.r, 1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].color.g, 0.0, 1e-5); + ASSERT_NEAR(markers.markers[0].color.b, 0.5, 1e-5); + ASSERT_NEAR(markers.markers[0].color.a, 0.7, 1e-5); + + // Check position + ASSERT_NEAR(markers.markers[0].pose.position.x, 3.0, 1e-5); + ASSERT_NEAR(markers.markers[0].pose.position.y, 4.0, 1e-5); + ASSERT_NEAR(markers.markers[0].pose.position.z, 0.0, 1e-5); + + // Check orientation + ASSERT_NEAR(markers.markers[0].pose.orientation.x, 0.0, 1e-5); + ASSERT_NEAR(markers.markers[0].pose.orientation.y, 0.0, 1e-5); + ASSERT_NEAR(markers.markers[0].pose.orientation.z, 0.70710678118, 1e-5); + ASSERT_NEAR(markers.markers[0].pose.orientation.w, 0.70710678118, 1e-5); + + // Check the marker shape (box = 4 vertices + close = 5 points) + ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_STRIP); + ASSERT_EQ(markers.markers[0].points.size(), 5); + ASSERT_NEAR(markers.markers[0].points[0].x, -1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[0].y, -2.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].y, -2.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[2].x, 1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[2].y, 2.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[3].x, -1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[3].y, 2.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[4].x, -1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[4].y, -2.0, 1e-5); + + b2DestroyWorld(world); +} + +// Test the bodyToMarkers method on a circle shape +TEST(DebugVizTest, testBodyToMarkersCircle) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + bodyDef.type = b2_dynamicBody; + bodyDef.position = {3.0f, 4.0f}; + bodyDef.rotation = b2MakeRot(M_PI_2); + b2BodyId body = b2CreateBody(world, &bodyDef); + + b2Circle circle; + circle.center = {2.0f, 3.0f}; + circle.radius = 0.2f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCircleShape(body, &shapeDef, &circle); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.0, 1.0); + ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers[0].type, markers.markers[0].SPHERE_LIST); + ASSERT_NEAR(markers.markers[0].scale.x, 0.4, 1e-5); + ASSERT_NEAR(markers.markers[0].scale.y, 0.4, 1e-5); + ASSERT_NEAR(markers.markers[0].scale.z, 0.01, 1e-5); + + b2DestroyWorld(world); +} + +// Test the bodyToMarkers method on a segment (edge) shape +TEST(DebugVizTest, testBodyToMarkersEdge) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId body = b2CreateBody(world, &bodyDef); + + b2Segment seg = {{0.5f, 1.5f}, {3.5f, 2.0f}}; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateSegmentShape(body, &shapeDef, &seg); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.0, 1.0); + ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); + ASSERT_EQ(markers.markers[0].points.size(), 2); + ASSERT_NEAR(markers.markers[0].points[0].x, 0.5, 1e-5); + ASSERT_NEAR(markers.markers[0].points[0].y, 1.5, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].x, 3.5, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].y, 2.0, 1e-5); + + b2DestroyWorld(world); +} + +// Test the bodyToMarkers method on an unsupported shape (body with no shapes) +TEST(DebugVizTest, testBodyToMarkersUnsupported) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId body = b2CreateBody(world, &bodyDef); + // No shapes added 窶・BodyToMarkers should produce no markers + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.0, 1.0); + ASSERT_EQ(markers.markers.size(), 0); + + b2DestroyWorld(world); +} + +// test bodyToMarkers with a body with multiple segment shapes +TEST(DebugVizTest, testBodyToMarkersMultifixture) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId body = b2CreateBody(world, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment edge1 = {{0.0f, 1.0f}, {1.0f, 2.0f}}; + b2Segment edge2 = {{-1.0f, 3.0f}, {5.0f, 7.0f}}; + b2CreateSegmentShape(body, &shapeDef, &edge1); + b2CreateSegmentShape(body, &shapeDef, &edge2); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.0, 1.0); + // Both segments get combined into one LINE_LIST marker + ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); + ASSERT_EQ(markers.markers[0].points.size(), 4); + + b2DestroyWorld(world); +} + +// test bodyToMarkers with multiple bodies +TEST(DebugVizTest, testBodyToMarkersMultibody) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId body = b2CreateBody(world, &bodyDef); + b2BodyId body2 = b2CreateBody(world, &bodyDef); + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2Segment edge1 = {{0.0f, 1.0f}, {1.0f, 2.0f}}; + b2Segment edge2 = {{-1.0f, 3.0f}, {5.0f, 7.0f}}; + b2CreateSegmentShape(body, &shapeDef, &edge1); + b2CreateSegmentShape(body2, &shapeDef, &edge2); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body, 1.0, + 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get().BodyToMarkers(markers, body2, 1.0, + 0.0, 0.0, 1.0); + // Both bodies' segments extend the same LINE_LIST marker + ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); + ASSERT_EQ(markers.markers[0].points.size(), 4); + // First body edge + ASSERT_NEAR(markers.markers[0].points[0].x, 0.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[0].y, 1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[1].y, 2.0, 1e-5); + // Second body edge + ASSERT_NEAR(markers.markers[0].points[2].x, -1.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[2].y, 3.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[3].x, 5.0, 1e-5); + ASSERT_NEAR(markers.markers[0].points[3].y, 7.0, 1e-5); + + b2DestroyWorld(world); +} + +// test JointToMarkers with multiple weld joints +TEST(DebugVizTest, testJointToMarkersMultiJoint) { + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId b1 = b2CreateBody(world, &bodyDef); + b2BodyId b2body = b2CreateBody(world, &bodyDef); + + b2WeldJointDef jd1 = b2DefaultWeldJointDef(); + jd1.bodyIdA = b1; + jd1.bodyIdB = b2body; + jd1.localAnchorA = {0.0f, 0.0f}; + jd1.localAnchorB = {0.0f, 0.0f}; + b2JointId j1 = b2CreateWeldJoint(world, &jd1); + + b2WeldJointDef jd2 = b2DefaultWeldJointDef(); + jd2.bodyIdA = b1; + jd2.bodyIdB = b2body; + jd2.localAnchorA = {1.0f, 2.0f}; + jd2.localAnchorB = {3.0f, 4.0f}; + b2JointId j2 = b2CreateWeldJoint(world, &jd2); + + visualization_msgs::MarkerArray markers; + flatland_server::DebugVisualization::Get().JointToMarkers(markers, j1, 0.1, + 0.2, 0.3, 0.4); + flatland_server::DebugVisualization::Get().JointToMarkers(markers, j2, 0.5, + 0.6, 0.7, 0.8); + ASSERT_EQ(markers.markers.size(), 4); + + // Check the 1st marker (j1: all anchors at origin) + ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); + ASSERT_EQ(markers.markers[0].points.size(), 6); + for (unsigned int i = 0; i < 6; i++) { + ASSERT_FLOAT_EQ(markers.markers[0].points[i].x, 0.0) << "index: " << i; + ASSERT_FLOAT_EQ(markers.markers[0].points[i].y, 0.0) << "index: " << i; + } + ASSERT_FLOAT_EQ(markers.markers[0].color.r, 0.1); + ASSERT_FLOAT_EQ(markers.markers[0].color.g, 0.2); + ASSERT_FLOAT_EQ(markers.markers[0].color.b, 0.3); + ASSERT_FLOAT_EQ(markers.markers[0].color.a, 0.4); + + // Check the 2nd marker (j1 CUBE_LIST: anchors at origin) + ASSERT_EQ(markers.markers[1].type, markers.markers[1].CUBE_LIST); + ASSERT_EQ(markers.markers[1].points.size(), 4); + for (unsigned int i = 0; i < 4; i++) { + ASSERT_FLOAT_EQ(markers.markers[1].points[i].x, 0.0) << "index: " << i; + ASSERT_FLOAT_EQ(markers.markers[1].points[i].y, 0.0) << "index: " << i; + } + + // Check the 3rd marker (j2 LINE_LIST: bodyA->anchorA, bodyB->anchorB, anchorA->anchorB) + // All bodies at (0,0); anchorA=(1,2), anchorB=(3,4) + ASSERT_EQ(markers.markers[2].type, markers.markers[2].LINE_LIST); + ASSERT_EQ(markers.markers[2].points.size(), 6); + ASSERT_FLOAT_EQ(markers.markers[2].points[0].x, 0.0); // bodyA pos + ASSERT_FLOAT_EQ(markers.markers[2].points[0].y, 0.0); + ASSERT_FLOAT_EQ(markers.markers[2].points[1].x, 1.0); // anchorA world + ASSERT_FLOAT_EQ(markers.markers[2].points[1].y, 2.0); + ASSERT_FLOAT_EQ(markers.markers[2].points[2].x, 0.0); // bodyB pos + ASSERT_FLOAT_EQ(markers.markers[2].points[2].y, 0.0); + ASSERT_FLOAT_EQ(markers.markers[2].points[3].x, 3.0); // anchorB world + ASSERT_FLOAT_EQ(markers.markers[2].points[3].y, 4.0); + ASSERT_FLOAT_EQ(markers.markers[2].points[4].x, 1.0); // anchorA + ASSERT_FLOAT_EQ(markers.markers[2].points[4].y, 2.0); + ASSERT_FLOAT_EQ(markers.markers[2].points[5].x, 3.0); // anchorB + ASSERT_FLOAT_EQ(markers.markers[2].points[5].y, 4.0); + + // Check the 4th marker (j2 CUBE_LIST) + ASSERT_EQ(markers.markers[3].type, markers.markers[3].CUBE_LIST); + ASSERT_EQ(markers.markers[3].points.size(), 4); + ASSERT_FLOAT_EQ(markers.markers[3].points[0].x, 1.0); // anchorA + ASSERT_FLOAT_EQ(markers.markers[3].points[0].y, 2.0); + ASSERT_FLOAT_EQ(markers.markers[3].points[1].x, 3.0); // anchorB + ASSERT_FLOAT_EQ(markers.markers[3].points[1].y, 4.0); + ASSERT_FLOAT_EQ(markers.markers[3].points[2].x, 0.0); // bodyA pos + ASSERT_FLOAT_EQ(markers.markers[3].points[2].y, 0.0); + ASSERT_FLOAT_EQ(markers.markers[3].points[3].x, 0.0); // bodyB pos + ASSERT_FLOAT_EQ(markers.markers[3].points[3].y, 0.0); + + b2DestroyWorld(world); +} + +// A helper class to accept MarkerArray message callbacks +struct MarkerArraySubscriptionHelper { + visualization_msgs::MarkerArray markers_; + int count_; + + MarkerArraySubscriptionHelper() : count_(0) {} + + void callback(const visualization_msgs::MarkerArrayConstPtr& msg) { + ++count_; + ROS_INFO("GOT ONE"); + markers_ = visualization_msgs::MarkerArray(*msg); + } + + bool waitForMessageCount(int count) { + ros::Rate rate(10); + for (unsigned int i = 0; i < 20; i++) { + ros::spinOnce(); + if (count_ >= count) return true; + rate.sleep(); + } + return false; + } +}; + +// Test publish/reset of visualization markers +TEST(DebugVizTest, testPublishMarkers) { + flatland_server::Timekeeper timekeeper; + timekeeper.SetMaxStepSize(0.01); + + b2WorldId world = MakeTestWorld(); + + b2BodyDef bodyDef = b2DefaultBodyDef(); + b2BodyId body = b2CreateBody(world, &bodyDef); + b2BodyId body2 = b2CreateBody(world, &bodyDef); + + b2Circle circle; + circle.center = {2.0f, 3.0f}; + circle.radius = 0.2f; + b2ShapeDef shapeDef = b2DefaultShapeDef(); + b2CreateCircleShape(body, &shapeDef, &circle); + + b2WeldJointDef jd = b2DefaultWeldJointDef(); + jd.bodyIdA = body; + jd.bodyIdB = body2; + jd.localAnchorA = {0.0f, 0.0f}; + jd.localAnchorB = {0.0f, 0.0f}; + b2JointId joint = b2CreateWeldJoint(world, &jd); + + ros::NodeHandle nh; + MarkerArraySubscriptionHelper helper; + ros::Subscriber sub = + nh.subscribe("/debug_visualization_test/debug/example", 0, + &MarkerArraySubscriptionHelper::callback, &helper); + + flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, + 0.0, 0.0, 1.0); + + EXPECT_EQ(flatland_server::DebugVisualization::Get().topics_.size(), 1); + ros::spinOnce(); + EXPECT_EQ(helper.count_, 0); + EXPECT_EQ(flatland_server::DebugVisualization::Get() + .topics_["example"] + .needs_publishing, + true); + EXPECT_EQ(sub.getNumPublishers(), 1); + + flatland_server::DebugVisualization::Get().Publish(timekeeper); + EXPECT_TRUE(helper.waitForMessageCount(1)); + EXPECT_EQ(helper.markers_.markers.size(), 1); + + flatland_server::DebugVisualization::Get().Publish(timekeeper); + EXPECT_TRUE(helper.waitForMessageCount(1)); + EXPECT_EQ(1, helper.markers_.markers.size()); + + flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, + 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get().Visualize("example", body, 1.0, + 0.0, 0.0, 1.0); + // inserts two markers + flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0, + 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get().Publish(timekeeper); + EXPECT_TRUE(helper.waitForMessageCount(2)); + EXPECT_EQ(5, helper.markers_.markers.size()); + + flatland_server::DebugVisualization::Get().Reset("example"); + flatland_server::DebugVisualization::Get().Publish(timekeeper); + EXPECT_TRUE(helper.waitForMessageCount(2)); + + flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0, + 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get().Publish(timekeeper); + EXPECT_TRUE(helper.waitForMessageCount(3)); + EXPECT_EQ(2, helper.markers_.markers.size()); + + b2DestroyWorld(world); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "debug_visualization_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/dummy_model_plugin_test.cpp b/flatland_server/test/dummy_model_plugin_test.cpp index 804eb68e..0f42be84 100644 --- a/flatland_server/test/dummy_model_plugin_test.cpp +++ b/flatland_server/test/dummy_model_plugin_test.cpp @@ -1,86 +1,86 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name laser_test.cpp - * @brief test laser plugin - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -/** - * Test the pluginlib is configured correctly so that the model can be - * discovered - */ -TEST(DummyModelPluginTest, pluginlib_load_test) { - pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); - - try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::DummyModelPlugin"); - - YAML::Node n = YAML::Node(); - n["dummy_param_float"] = 0.123456; - n["dummy_param_string"] = "dummy_test_123456"; - n["dummy_param_int"] = 123456; - - flatland_server::CollisionFilterRegistry cfr; - flatland_server::Model model(nullptr, &cfr, "", ""); - - plugin->Initialize("DummyModelPlugin", "DummyModelPluginTest", &model, n); - } catch (pluginlib::PluginlibException& e) { - FAIL() << "Failed to load Dummy Model Plugin. " << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "dummy_model_plugin_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name laser_test.cpp + * @brief test laser plugin + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +/** + * Test the pluginlib is configured correctly so that the model can be + * discovered + */ +TEST(DummyModelPluginTest, pluginlib_load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::ModelPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::DummyModelPlugin"); + + YAML::Node n = YAML::Node(); + n["dummy_param_float"] = 0.123456; + n["dummy_param_string"] = "dummy_test_123456"; + n["dummy_param_int"] = 123456; + + flatland_server::CollisionFilterRegistry cfr; + flatland_server::Model model(nullptr, &cfr, "", ""); + + plugin->Initialize("DummyModelPlugin", "DummyModelPluginTest", &model, n); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load Dummy Model Plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "dummy_model_plugin_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/dummy_world_plugin_test.cpp b/flatland_server/test/dummy_world_plugin_test.cpp index 5f52dcc1..0e7398bb 100644 --- a/flatland_server/test/dummy_world_plugin_test.cpp +++ b/flatland_server/test/dummy_world_plugin_test.cpp @@ -1,80 +1,80 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name dummy_world_plugin_test.cpp - * @brief test world plugin - * @author Yi Ren - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace flatland_server; - -TEST(DummyWorldPluginTest, pluginlib_load_test) { - pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::WorldPlugin"); - - try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::DummyWorldPlugin"); - - YAML::Node n = YAML::Node(); - YamlReader reader = YamlReader(); - plugin->Initialize(NULL, "DummyWorldPluginName", "DummyWorldPluginType", n, - reader); - } catch (pluginlib::PluginlibException& e) { - FAIL() << "Failed to load Dummy World Plugin. " << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "dummy_world_plugin_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name dummy_world_plugin_test.cpp + * @brief test world plugin + * @author Yi Ren + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace flatland_server; + +TEST(DummyWorldPluginTest, pluginlib_load_test) { + pluginlib::ClassLoader loader( + "flatland_server", "flatland_server::WorldPlugin"); + + try { + boost::shared_ptr plugin = + loader.createInstance("flatland_plugins::DummyWorldPlugin"); + + YAML::Node n = YAML::Node(); + YamlReader reader = YamlReader(); + plugin->Initialize(NULL, "DummyWorldPluginName", "DummyWorldPluginType", n, + reader); + } catch (pluginlib::PluginlibException& e) { + FAIL() << "Failed to load Dummy World Plugin. " << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "dummy_world_plugin_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/geometry_test.cpp b/flatland_server/test/geometry_test.cpp index c101be87..cda83dba 100644 --- a/flatland_server/test/geometry_test.cpp +++ b/flatland_server/test/geometry_test.cpp @@ -1,102 +1,102 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name geometry_test.cpp - * @brief Test geometry - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/geometry.h" -#include -#include - -// Test the CreateTransform method -TEST(TestSuite, testCreateTransform) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(2.0, 1.0, 1.1); - - EXPECT_NEAR(rt.dx, 2.0, 1e-5); - EXPECT_NEAR(rt.dy, 1.0, 1e-5); - EXPECT_NEAR(rt.cos, cosf(1.1), 1e-5); - EXPECT_NEAR(rt.sin, sinf(1.1), 1e-5); -} - -// Test the Transform method, translation -TEST(TestSuite, testTransformTranslate) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(2.0, 1.5, 0.0); - - b2Vec2 in(1.0, 2.0); - b2Vec2 out = flatland_server::Geometry::Transform(in, rt); - - EXPECT_NEAR(out.x, 3.0, 1e-5); - EXPECT_NEAR(out.y, 3.5, 1e-5); -} - -// Test the Transform method, rotation -TEST(TestSuite, testTransformRotate) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(0.0, 0.0, M_PI_2); - - b2Vec2 in(1.0, 2.0); - b2Vec2 out = flatland_server::Geometry::Transform(in, rt); - - EXPECT_NEAR(out.x, -2.0, 1e-5); - EXPECT_NEAR(out.y, 1.0, 1e-5); -} - -// Test the Transform method, translation + rotation -TEST(TestSuite, testTransformTranslateAndRotate) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(1.0, 0.5, -M_PI); - - b2Vec2 in(-1.0, 1.5); - b2Vec2 out = flatland_server::Geometry::Transform(in, rt); - - EXPECT_NEAR(out.x, 2.0, 1e-5); - EXPECT_NEAR(out.y, -1.0, 1e-5); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name geometry_test.cpp + * @brief Test geometry + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/geometry.h" +#include +#include + +// Test the CreateTransform method +TEST(TestSuite, testCreateTransform) { + flatland_server::RotateTranslate rt = + flatland_server::Geometry::CreateTransform(2.0, 1.0, 1.1); + + EXPECT_NEAR(rt.dx, 2.0, 1e-5); + EXPECT_NEAR(rt.dy, 1.0, 1e-5); + EXPECT_NEAR(rt.cos, cosf(1.1), 1e-5); + EXPECT_NEAR(rt.sin, sinf(1.1), 1e-5); +} + +// Test the Transform method, translation +TEST(TestSuite, testTransformTranslate) { + flatland_server::RotateTranslate rt = + flatland_server::Geometry::CreateTransform(2.0, 1.5, 0.0); + + b2Vec2 in(1.0, 2.0); + b2Vec2 out = flatland_server::Geometry::Transform(in, rt); + + EXPECT_NEAR(out.x, 3.0, 1e-5); + EXPECT_NEAR(out.y, 3.5, 1e-5); +} + +// Test the Transform method, rotation +TEST(TestSuite, testTransformRotate) { + flatland_server::RotateTranslate rt = + flatland_server::Geometry::CreateTransform(0.0, 0.0, M_PI_2); + + b2Vec2 in(1.0, 2.0); + b2Vec2 out = flatland_server::Geometry::Transform(in, rt); + + EXPECT_NEAR(out.x, -2.0, 1e-5); + EXPECT_NEAR(out.y, 1.0, 1e-5); +} + +// Test the Transform method, translation + rotation +TEST(TestSuite, testTransformTranslateAndRotate) { + flatland_server::RotateTranslate rt = + flatland_server::Geometry::CreateTransform(1.0, 0.5, -M_PI); + + b2Vec2 in(-1.0, 1.5); + b2Vec2 out = flatland_server::Geometry::Transform(in, rt); + + EXPECT_NEAR(out.x, 2.0, 1e-5); + EXPECT_NEAR(out.y, -1.0, 1e-5); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp index 2abd1782..a930643d 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -1,1004 +1,942 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name load_world_test.cpp - * @brief Testing the load world functionality - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; - -class LoadWorldTest : public ::testing::Test { - protected: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - World *w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - // to test that the world instantiation will fail, and the exception - // message matches the given regex string - void test_yaml_fail(std::string regex_str) { - // do a regex match against error messages - std::cmatch match; - std::regex regex(regex_str); - - try { - w = World::MakeWorld(world_yaml.string()); - ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const Exception &e) { - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { - ADD_FAILURE() << "Expected flatland_server::Exception, another exception " - "was caught instead: " - << e.what(); - } - } - - bool float_cmp(double n1, double n2) { - bool ret = fabs(n1 - n2) < 1e-5; - return ret; - } - - // return the id if found, -1 otherwise - int does_edge_exist(const b2EdgeShape &edge, - const std::vector> &edges) { - for (unsigned int i = 0; i < edges.size(); i++) { - auto e = edges[i]; - if ((float_cmp(edge.m_vertex1.x, e.first.x) && - float_cmp(edge.m_vertex1.y, e.first.y) && - float_cmp(edge.m_vertex2.x, e.second.x) && - float_cmp(edge.m_vertex2.y, e.second.y)) || - - (float_cmp(edge.m_vertex1.x, e.second.x) && - float_cmp(edge.m_vertex1.y, e.second.y) && - float_cmp(edge.m_vertex2.x, e.first.x) && - float_cmp(edge.m_vertex2.y, e.first.y))) { - return i; - } - } - - return -1; - } - - // checks if one list of edges completely matches the content - // of the other list - bool do_edges_exactly_match( - const std::vector &edges1, - const std::vector> &edges2) { - std::vector> edges_cpy = edges2; - for (unsigned int i = 0; i < edges1.size(); i++) { - auto e = edges1[i]; - int ret_idx = does_edge_exist(e, edges_cpy); - - if (ret_idx < 0) { - b2Vec2 v1_tf = e.m_vertex1; - b2Vec2 v2_tf = e.m_vertex2; - return false; - } - - edges_cpy.erase(edges_cpy.begin() + ret_idx); - } - - if (edges_cpy.size() == 0) { - return true; - } else { - return false; - } - } - - bool ColorEq(const Color &c1, const Color &c2) { - if (c1 != c2) { - printf("Color Actual:[%f,%f,%f,%f] != Expected:[%f,%f,%f,%f]\n", c1.r, - c1.g, c1.b, c1.a, c2.r, c2.g, c2.b, c2.a); - return false; - } - return true; - } - - bool BodyEq(Body *body, const std::string name, b2BodyType type, - const std::array &pose, - const std::array &color, double linear_damping, - double angular_damping) { - b2Vec2 t = body->physics_body_->GetPosition(); - double a = body->physics_body_->GetAngle(); - - if (name != body->name_) { - printf("Name Actual:%s != Expected:%s\n", body->name_.c_str(), - name.c_str()); - return false; - } - - if (type != body->physics_body_->GetType()) { - /* - enum b2BodyType - { - b2_staticBody = 0, - b2_kinematicBody, - b2_dynamicBody - }; - */ - printf("Body type Actual:%d != Expected:%d\n", - body->physics_body_->GetType(), type); - return false; - } - - if (!float_cmp(t.x, pose[0]) || !float_cmp(t.y, pose[1]) || - !float_cmp(a, pose[2])) { - printf("Pose Actual:[%f,%f,%f] != Expected:[%f,%f,%f]\n", t.x, t.y, a, - pose[0], pose[1], pose[2]); - return false; - } - - if (!ColorEq(body->color_, Color(color))) { - return false; - } - - if (!float_cmp(linear_damping, body->physics_body_->GetLinearDamping())) { - printf("Linear Damping Actual:%f != Expected:%f\n", - body->physics_body_->GetLinearDamping(), linear_damping); - return false; - } - - if (!float_cmp(angular_damping, body->physics_body_->GetAngularDamping())) { - printf("Angular Damping Actual %f != Expected:%f\n", - body->physics_body_->GetAngularDamping(), angular_damping); - return false; - } - - return true; - } - - bool CircleEq(b2Fixture *f, double x, double y, double r) { - if (f->GetShape()->GetType() != b2Shape::e_circle) { - /* - enum Type - { - e_circle = 0, - e_edge = 1, - e_polygon = 2, - e_chain = 3, - e_typeCount = 4 - }; - */ - printf("Shape is not of type b2Shape::e_circle, Actual=%d\n", - f->GetShape()->GetType()); - return false; - } - - b2CircleShape *s = dynamic_cast(f->GetShape()); - - if (!float_cmp(r, s->m_radius) || !float_cmp(x, s->m_p.x) || - !float_cmp(y, s->m_p.y)) { - printf("Actual:[x=%f,y=%f,r=%f] != Expected:[%f,%f,%f] \n", s->m_p.x, - s->m_p.y, s->m_radius, x, y, r); - return false; - } - return true; - } - - bool PolygonEq(b2Fixture *f, std::vector> points) { - if (f->GetShape()->GetType() != b2Shape::e_polygon) { - /* - enum Type - { - e_circle = 0, - e_edge = 1, - e_polygon = 2, - e_chain = 3, - e_typeCount = 4 - }; - */ - printf("Shape is not of type b2Shape::e_polygon, Actual=%d\n", - f->GetShape()->GetType()); - return false; - } - - b2PolygonShape *s = dynamic_cast(f->GetShape()); - int cnt = s->m_count; - if (cnt != points.size()) { - printf("Number of points Actual:%d != Expected:%lu\n", cnt, - points.size()); - return false; - } - - auto pts = points; - - for (unsigned int i = 0; i < cnt; i++) { - const b2Vec2 p = s->m_vertices[i]; - - bool found_match = false; - int j; - for (j = 0; j < pts.size(); j++) { - if (!float_cmp(p.x, points[i].first) || - !float_cmp(p.y, points[i].second)) { - found_match = true; - break; - } - } - - if (!found_match) { - // cannot find a matching point, print the expected and actual points - printf("Actual: ["); - for (int k = 0; k < cnt; k++) { - printf("[%f,%f],", s->m_vertices[k].x, s->m_vertices[k].y); - } - - printf("] != Expected: ["); - for (int k = 0; k < points.size(); k++) { - printf("[%f,%f],", points[k].first, points[k].second); - } - printf("]\n"); - - return false; - } - - pts.erase(pts.begin() + j); - } - - if (pts.size() == 0) { - return true; - } else { - return false; - } - } - - std::vector GetBodyFixtures(Body *body) { - std::vector fixtures; - - b2Body *b = body->physics_body_; - for (b2Fixture *f = b->GetFixtureList(); f; f = f->GetNext()) { - fixtures.push_back(f); - } - - std::reverse(fixtures.begin(), fixtures.end()); - - return fixtures; - } - - bool FixtureEq(b2Fixture *f, bool is_sensor, int group_index, - uint16_t category_bits, uint16_t mask_bits, double density, - double friction, double restitution) { - if (f->IsSensor() != is_sensor) { - printf("is_sensor Actual:%d != Expected:%d\n", f->IsSensor(), is_sensor); - return false; - } - - if (f->GetFilterData().groupIndex != group_index) { - printf("group_index Actual:%d != Expected:%d\n", - f->GetFilterData().groupIndex, group_index); - return false; - } - - if (f->GetFilterData().categoryBits != category_bits) { - printf("category_bits Actual:0x%X != Expected:0x%X\n", - f->GetFilterData().categoryBits, category_bits); - return false; - } - - if (f->GetFilterData().maskBits != mask_bits) { - printf("mask_bits Actual:0x%X != Expected:0x%X\n", - f->GetFilterData().maskBits, mask_bits); - return false; - } - - if (!float_cmp(f->GetDensity(), density)) { - printf("density Actual:%f != Expected:%f\n", f->GetDensity(), density); - return false; - } - - if (!float_cmp(f->GetFriction(), friction)) { - printf("friction Actual:%f != Expected:%f\n", f->GetFriction(), friction); - return false; - } - - if (!float_cmp(f->GetRestitution(), restitution)) { - printf("restitution Actual:%f != Expected:%f\n", f->GetRestitution(), - restitution); - return false; - } - - return true; - } - - bool JointEq(Joint *joint, const std::string name, - const std::array &color, Body *body_A, - const std::array &anchor_A, Body *body_B, - const std::array &anchor_B, bool collide_connected) { - b2Joint *j = joint->physics_joint_; - - if (name != joint->name_) { - printf("Name Actual:%s != Expected:%s\n", joint->name_.c_str(), - name.c_str()); - return false; - } - - if (!ColorEq(joint->color_, color)) { - return false; - } - - if (j->GetBodyA() != body_A->physics_body_) { - printf("BodyA ptr Actual %p != Expected:%p\n", - joint->physics_joint_->GetBodyA(), body_A->physics_body_); - return false; - } - - if (j->GetBodyB() != body_B->physics_body_) { - printf("BodyB ptr Actual %p != Expected:%p\n", j->GetBodyB(), - body_B->physics_body_); - return false; - } - - // GetAnchor returns world coordinates, we want to verify against - // local coordinates - b2Vec2 local_anchor_A = j->GetAnchorA() - j->GetBodyA()->GetPosition(); - b2Vec2 local_anchor_B = j->GetAnchorB() - j->GetBodyB()->GetPosition(); - - if (!float_cmp(local_anchor_A.x, anchor_A[0]) || - !float_cmp(local_anchor_A.y, anchor_A[1])) { - printf("Anchor A Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_A.x, - local_anchor_A.y, anchor_A[0], anchor_A[1]); - return false; - } - - if (!float_cmp(local_anchor_B.x, anchor_B[0]) || - !float_cmp(local_anchor_B.y, anchor_B[1])) { - printf("Anchor B Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_B.x, - local_anchor_B.y, anchor_B[0], anchor_B[1]); - return false; - } - - if (collide_connected != j->GetCollideConnected()) { - printf("Collide connected Actual:%d != Expected:%d\n", - j->GetCollideConnected(), collide_connected); - return false; - } - - return true; - } - - bool WeldEq(Joint *joint, double angle, double freq, double damping) { - b2WeldJoint *j = dynamic_cast(joint->physics_joint_); - - if (j->GetType() != e_weldJoint) { - /* - enum b2JointType - { - e_unknownJoint, --> C++ should defaults initialize at zero? - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_ropeJoint, - e_motorJoint - }; - */ - printf("Joint type Actual:%d != Expected:%d(weld joint)\n", j->GetType(), - e_weldJoint); - return false; - } - - if (!float_cmp(angle, j->GetReferenceAngle())) { - printf("Angle Actual:%f != Expected:%f\n", angle, j->GetReferenceAngle()); - return false; - } - - if (!float_cmp(freq, j->GetFrequency())) { - printf("Frequency Actual:%f != Expected:%f\n", freq, j->GetFrequency()); - return false; - } - - if (!float_cmp(damping, j->GetDampingRatio())) { - printf("Damping Actual:%f != Expected:%f\n", damping, - j->GetDampingRatio()); - return false; - } - - return true; - } - - bool RevoluteEq(Joint *joint, bool is_limit_enabled, - const std::array limits) { - b2RevoluteJoint *j = dynamic_cast(joint->physics_joint_); - - if (j->GetType() != e_revoluteJoint) { - /* - enum b2JointType - { - e_unknownJoint, --> C++ defaults initialize at zero? - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_ropeJoint, - e_motorJoint - }; - */ - printf("Joint type Actual:%d != Expected:%d(revolute joint)\n", - j->GetType(), e_revoluteJoint); - return false; - } - - if (is_limit_enabled != j->IsLimitEnabled()) { - printf("Limits enabled Actual:%d != Expected:%d\n", is_limit_enabled, - j->IsLimitEnabled()); - return false; - } - - if (is_limit_enabled && (!float_cmp(limits[0], j->GetLowerLimit()) || - !float_cmp(limits[1], j->GetUpperLimit()))) { - printf("Limits Actual:[%f,%f] != Expected:[%f,%f]\n", j->GetLowerLimit(), - j->GetLowerLimit(), limits[0], limits[1]); - return false; - } - - return true; - } -}; - -/** - * This test loads the world, layers, models from the given world - * yaml file and checks that all configurations, data, and calculations are - * correct after instantiation - */ -TEST_F(LoadWorldTest, simple_test_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - w = World::MakeWorld(world_yaml.string()); - - EXPECT_EQ(w->physics_velocity_iterations_, 11); - EXPECT_EQ(w->physics_position_iterations_, 12); - - ASSERT_EQ(w->layers_.size(), 4); - - // check that layer 0 settings are loaded correctly - EXPECT_STREQ(w->layers_[0]->name_.c_str(), "2d"); - ASSERT_EQ(w->layers_[0]->names_.size(), 1); - EXPECT_STREQ(w->layers_[0]->names_[0].c_str(), "2d"); - EXPECT_EQ(w->layers_[0]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[0]->body_, "2d", b2_staticBody, - {0.05, -0.05, 1.57}, {0, 1, 0, 0.675}, 0, 0)); - EXPECT_EQ(w->cfr_.LookUpLayerId("2d"), 0); - EXPECT_EQ(w->cfr_.GetCategoryBits(w->layers_[0]->names_), 0b1); - - // check that layer 1 settings are loaded correctly - EXPECT_STREQ(w->layers_[1]->name_.c_str(), "3d"); - ASSERT_EQ(w->layers_[1]->names_.size(), 3); - EXPECT_STREQ(w->layers_[1]->names_[0].c_str(), "3d"); - EXPECT_STREQ(w->layers_[1]->names_[1].c_str(), "4d"); - EXPECT_STREQ(w->layers_[1]->names_[2].c_str(), "5d"); - EXPECT_EQ(w->layers_[1]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[1]->body_, "3d", b2_staticBody, {0.0, 0.0, 0.0}, - {1, 1, 1, 1}, 0, 0)); - EXPECT_EQ(w->cfr_.LookUpLayerId("3d"), 1); - EXPECT_EQ(w->cfr_.LookUpLayerId("4d"), 2); - EXPECT_EQ(w->cfr_.LookUpLayerId("5d"), 3); - EXPECT_EQ(w->cfr_.GetCategoryBits(w->layers_[1]->names_), 0b1110); - - // check that layer 2 settings are loaded correctly - EXPECT_STREQ(w->layers_[2]->name_.c_str(), "lines"); - ASSERT_EQ(w->layers_[2]->names_.size(), 1); - EXPECT_STREQ(w->layers_[2]->names_[0].c_str(), "lines"); - EXPECT_EQ(w->layers_[2]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[2]->body_, "lines", b2_staticBody, - {-1.20, -5, 1.23}, {1, 1, 1, 1}, 0, 0)); - EXPECT_EQ(w->cfr_.LookUpLayerId("lines"), 4); - - // check that bitmap is transformed correctly. This involves flipping the y - // coordinates and apply the resolution. Note that the translation and - // rotation is performed internally by Box2D - std::vector> layer0_expected_edges = { - std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.25, 0.25)), - std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.20, 0.20)), - std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.15, 0.15)), - std::pair(b2Vec2(0.10, 0.10), b2Vec2(0.15, 0.10)), - std::pair(b2Vec2(0.05, 0.05), b2Vec2(0.20, 0.05)), - std::pair(b2Vec2(0.00, 0.00), b2Vec2(0.25, 0.00)), - - std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.00, 0.00)), - std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.05, 0.05)), - std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.10, 0.10)), - std::pair(b2Vec2(0.15, 0.15), b2Vec2(0.15, 0.10)), - std::pair(b2Vec2(0.20, 0.20), b2Vec2(0.20, 0.05)), - std::pair(b2Vec2(0.25, 0.25), b2Vec2(0.25, 0.00))}; - - std::vector layer0_edges; - for (b2Fixture *f = w->layers_[0]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { - b2EdgeShape e = *(dynamic_cast(f->GetShape())); - layer0_edges.push_back(e); - - // check that collision groups are correctly assigned - ASSERT_EQ(f->GetFilterData().categoryBits, 0x1); - ASSERT_EQ(f->GetFilterData().maskBits, 0x1); - } - EXPECT_EQ(layer0_edges.size(), layer0_expected_edges.size()); - EXPECT_TRUE(do_edges_exactly_match(layer0_edges, layer0_expected_edges)); - - // layer[1] has origin of [0, 0, 0], so there should be no transform, just - // apply the inversion of y coordinates and scale by resolution - std::vector> layer1_expected_edges = { - std::pair(b2Vec2(0.0, 7.5), b2Vec2(1.5, 7.5)), - std::pair(b2Vec2(0.0, 7.5), b2Vec2(0.0, 4.5)), - std::pair(b2Vec2(0.0, 4.5), b2Vec2(4.5, 4.5)), - std::pair(b2Vec2(4.5, 4.5), b2Vec2(4.5, 1.5)), - std::pair(b2Vec2(6.0, 3.0), b2Vec2(6.0, 6.0)), - std::pair(b2Vec2(6.0, 6.0), b2Vec2(1.5, 6.0)), - std::pair(b2Vec2(1.5, 7.5), b2Vec2(1.5, 6.0)), - std::pair(b2Vec2(3.0, 3.0), b2Vec2(6.0, 3.0)), - std::pair(b2Vec2(3.0, 0.0), b2Vec2(3.0, 3.0)), - std::pair(b2Vec2(1.5, 1.5), b2Vec2(4.5, 1.5)), - std::pair(b2Vec2(1.5, 0.0), b2Vec2(3.0, 0.0)), - std::pair(b2Vec2(1.5, 1.5), b2Vec2(1.5, 0.0)), - std::pair(b2Vec2(6.0, 1.5), b2Vec2(7.5, 1.5)), - std::pair(b2Vec2(6.0, 1.5), b2Vec2(6.0, 0.0)), - std::pair(b2Vec2(7.5, 1.5), b2Vec2(7.5, 0.0)), - std::pair(b2Vec2(6.0, 0.0), b2Vec2(7.5, 0.0))}; - - std::vector layer1_edges; - for (b2Fixture *f = w->layers_[1]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { - b2EdgeShape e = *(dynamic_cast(f->GetShape())); - layer1_edges.push_back(e); - - // check that collision groups are correctly assigned - ASSERT_EQ(f->GetFilterData().categoryBits, 0b1110); - ASSERT_EQ(f->GetFilterData().maskBits, 0b1110); - } - EXPECT_EQ(layer1_edges.size(), layer1_expected_edges.size()); - EXPECT_TRUE(do_edges_exactly_match(layer1_edges, layer1_expected_edges)); - - // check layer[2] data - std::vector> layer2_expected_edges = { - std::pair(b2Vec2(0.1, 0.2), b2Vec2(0.3, 0.4)), - std::pair(b2Vec2(-0.1, -0.2), b2Vec2(-0.3, -0.4)), - std::pair(b2Vec2(0.01, 0.02), b2Vec2(0.03, 0.04))}; - - std::vector layer2_edges; - for (b2Fixture *f = w->layers_[2]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { - b2EdgeShape e = *(dynamic_cast(f->GetShape())); - layer2_edges.push_back(e); - - // check that collision groups are correctly assigned - ASSERT_EQ(f->GetFilterData().categoryBits, 0b10000); - ASSERT_EQ(f->GetFilterData().maskBits, 0b10000); - } - EXPECT_EQ(layer2_edges.size(), layer2_expected_edges.size()); - EXPECT_TRUE(do_edges_exactly_match(layer2_edges, layer2_expected_edges)); - - // Check loaded model data - // Check model 0 - Model *m0 = w->models_[0]; - EXPECT_STREQ(m0->name_.c_str(), "turtlebot1"); - EXPECT_STREQ(m0->namespace_.c_str(), ""); - ASSERT_EQ(m0->bodies_.size(), 5); - ASSERT_EQ(m0->joints_.size(), 4); - - // check model 0 body 0 - EXPECT_TRUE(BodyEq(m0->bodies_[0], "base", b2_dynamicBody, {0, 0, 0}, - {1, 1, 0, 0.25}, 0.1, 0.125)); - auto fs = GetBodyFixtures(m0->bodies_[0]); - ASSERT_EQ(fs.size(), 2); - EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); - EXPECT_TRUE(CircleEq(fs[0], 0, 0, 1.777)); - EXPECT_TRUE(FixtureEq(fs[1], false, 0, 0xFFFF, 0xFFFF, 982.24, 0.59, 0.234)); - EXPECT_TRUE( - PolygonEq(fs[1], {{-0.1, 0.1}, {-0.1, -0.1}, {0.1, -0.1}, {0.1, 0.1}})); - - // check model 0 body 1 - EXPECT_TRUE(BodyEq(m0->bodies_[1], "left_wheel", b2_dynamicBody, {-1, 0, 0}, - {1, 0, 0, 0.25}, 0, 0)); - fs = GetBodyFixtures(m0->bodies_[1]); - ASSERT_EQ(fs.size(), 1); - EXPECT_TRUE(FixtureEq(fs[0], true, 0, 0b01, 0b01, 0, 0, 0)); - EXPECT_TRUE(PolygonEq( - fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); - - // check model 0 body 2 - EXPECT_TRUE(BodyEq(m0->bodies_[2], "right_wheel", b2_dynamicBody, {1, 0, 0}, - {0, 1, 0, 0.25}, 0, 0)); - fs = GetBodyFixtures(m0->bodies_[2]); - ASSERT_EQ(fs.size(), 1); - EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); - EXPECT_TRUE(PolygonEq( - fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); - - // check model 0 body 3 - EXPECT_TRUE(BodyEq(m0->bodies_[3], "tail", b2_dynamicBody, {0, 0, 0.52}, - {0, 0, 0, 0.5}, 0, 0)); - fs = GetBodyFixtures(m0->bodies_[3]); - ASSERT_EQ(fs.size(), 1); - EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b10, 0b10, 0, 0, 0)); - EXPECT_TRUE(PolygonEq(fs[0], {{-0.2, 0}, {-0.2, -5}, {0.2, -5}, {0.2, 0}})); - - // check model 0 body 4 - EXPECT_TRUE(BodyEq(m0->bodies_[4], "antenna", b2_dynamicBody, {0, 0.5, 0}, - {0.2, 0.4, 0.6, 1}, 0, 0)); - fs = GetBodyFixtures(m0->bodies_[4]); - ASSERT_EQ(fs.size(), 1); - EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b0, 0b0, 0, 0, 0)); - EXPECT_TRUE(CircleEq(fs[0], 0.01, 0.02, 0.25)); - - // Check loaded joint data - EXPECT_TRUE(JointEq(m0->joints_[0], "left_wheel_weld", {0.1, 0.2, 0.3, 0.4}, - m0->bodies_[0], {-1, 0}, m0->bodies_[1], {0, 0}, true)); - EXPECT_TRUE(WeldEq(m0->joints_[0], 1.57079633, 10, 0.5)); - - EXPECT_TRUE(JointEq(m0->joints_[1], "right_wheel_weld", {1, 1, 1, 0.5}, - m0->bodies_[0], {1, 0}, m0->bodies_[2], {0, 0}, false)); - EXPECT_TRUE(WeldEq(m0->joints_[1], 0, 0, 0)); - - EXPECT_TRUE(JointEq(m0->joints_[2], "tail_revolute", {1, 1, 1, 0.5}, - m0->bodies_[0], {0, 0}, m0->bodies_[3], {0, 0}, false)); - EXPECT_TRUE(RevoluteEq(m0->joints_[2], false, {})); - - EXPECT_TRUE(JointEq(m0->joints_[3], "antenna_revolute", {1, 1, 1, 0.5}, - m0->bodies_[0], {0, 0}, m0->bodies_[4], {0, 0}, true)); - EXPECT_TRUE(RevoluteEq(m0->joints_[3], true, {-0.002, 3.735})); - - // Check model 1 is same yaml file as model 1, simply do a simple sanity check - Model *m1 = w->models_[1]; - EXPECT_STREQ(m1->name_.c_str(), "turtlebot2"); - EXPECT_STREQ(m1->namespace_.c_str(), "robot2"); - ASSERT_EQ(m1->bodies_.size(), 5); - ASSERT_EQ(m1->joints_.size(), 4); - - // check the applied transformation just for the first body - EXPECT_TRUE(BodyEq(m1->bodies_[0], "base", b2_dynamicBody, {3, 4.5, 3.14159}, - {1, 1, 0, 0.25}, 0.1, 0.125)); - - // Check model 2 which is the chair - Model *m2 = w->models_[2]; - EXPECT_STREQ(m2->name_.c_str(), "chair1"); - ASSERT_EQ(m2->bodies_.size(), 1); - ASSERT_EQ(m2->joints_.size(), 0); - - // Check model 2 fixtures - EXPECT_TRUE(BodyEq(m2->bodies_[0], "chair", b2_staticBody, {1.2, 3.5, 2.123}, - {1, 1, 1, 0.5}, 0, 0)); - fs = GetBodyFixtures(m2->bodies_[0]); - ASSERT_EQ(fs.size(), 2); - EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b1100, 0b1100, 0, 0, 0)); - EXPECT_TRUE(CircleEq(fs[0], 0, 0, 1)); - - EXPECT_TRUE(FixtureEq(fs[1], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); - EXPECT_TRUE(CircleEq(fs[1], 0, 0, 0.2)); - - // Check model 3 which is the chair - Model *m3 = w->models_[3]; - EXPECT_STREQ(m3->name_.c_str(), "person1"); - ASSERT_EQ(m3->bodies_.size(), 1); - ASSERT_EQ(m3->joints_.size(), 0); - - // check the body only - EXPECT_TRUE(BodyEq(m3->bodies_[0], "body", b2_kinematicBody, {0, 1, 2}, - {0, 0.75, 0.75, 0.25}, 0, 0)); -} - -/** - * This test tries to loads a non-existent world yaml file. It should throw - * an exception - */ -TEST_F(LoadWorldTest, wrong_world_path) { - world_yaml = - this_file_dir / fs::path("load_world_tests/random_path/world.yaml"); - test_yaml_fail( - "Flatland YAML: File does not exist, " - "path=\".*/load_world_tests/random_path/world.yaml\".*"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_A/world.yaml"); - test_yaml_fail("Flatland YAML: Entry \"properties\" does not exist"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_B/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"color\" must have size of exactly 4 \\(in " - "\"layers\" index=0\\)"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_C/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " - "\\{\"random_param_1\", \"random_param_2\", \"random_param_3\"\\} \\(in " - "\"layers\"\\)"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_D/world.yaml"); - test_yaml_fail("Flatland YAML: Layer with name \"layer\" already exists"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_E/world.yaml"); - test_yaml_fail("Flatland YAML: Model with name \"turtlebot\" already exists"); -} - -/** - * This test tries to loads a invalid world yaml file. It should throw - * an exception. - */ -TEST_F(LoadWorldTest, world_invalid_F) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_F/world.yaml"); - test_yaml_fail( - "Flatland YAML: Unable to add 3 additional layer\\(s\\) \\{layer_15, " - "layer_16, layer_17\\}, current layers count is 14, max allowed is 16"); -} - -/** - * This test tries to loads valid world yaml file which in turn tries to - * load a invalid map yaml file. It should throw an exception. - */ -TEST_F(LoadWorldTest, map_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_A/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"origin\" does not exist \\(in layer " - "\"2d\"\\)"); -} - -/** - * This test tries to loads valid world yaml file which in turn load a map yaml - * file which then inturn tries to load a non-exists map image file. It should - * throw an exception - */ -TEST_F(LoadWorldTest, map_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_B/world.yaml"); - test_yaml_fail("Flatland YAML: Failed to load \".*\\.png\" in layer \"2d\""); -} - -/** - * This test tries to load a invalid map configuration, an exception should be - * thrown - */ -TEST_F(LoadWorldTest, map_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_C/world.yaml"); - test_yaml_fail( - "Flatland File: Failed to load \".*/map_invalid_C/random_file.dat\""); -} - -/** - * This test tries to load a invalid map configuration, an exception should be - * thrown - */ -TEST_F(LoadWorldTest, map_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_D/world.yaml"); - test_yaml_fail( - "Flatland File: Failed to read line segment from line 6, in file " - "\"map_lines.dat\""); -} - -/** - * This test tries to load a invalid map configuration, an exception should be - * thrown - */ -TEST_F(LoadWorldTest, map_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_E/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"scale\" does not exist \\(in layer \"lines\"\\)"); -} - -/**` - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_A/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"bodies\" must be a list \\(in model " - "\"turtlebot\"\\)"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_B/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"points\" must have size >= 3 \\(in model " - "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_C/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"anchor\" must have size of exactly 2 \\(in model " - "\"turtlebot\" joint \"right_wheel_weld\" \"bodies\" index=1\\)"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_D/world.yaml"); - test_yaml_fail( - "Flatland YAML: Cannot find body with name \"left_wheel_123\" in model " - "\"turtlebot\" joint \"left_wheel_weld\" \"bodies\" index=1"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_E/world.yaml"); - test_yaml_fail( - "Flatland YAML: Invalid footprint \"layers\" in model \"turtlebot\" body " - "\"left_wheel\" \"footprints\" index=0, \\{random_layer\\} layer\\(s\\) " - "does not exist"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_F) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_F/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " - "\\{\"random_paramter\"\\} \\(in model \"turtlebot\" body \"base\" " - "\"footprints\"\\)"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_G) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_G/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry body \"base\" contains unrecognized entry\\(s\\) " - "\\{\"random_paramter\"\\} \\(in model \"turtlebot\"\\)"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_H) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_H/world.yaml"); - test_yaml_fail( - "Flatland YAML: Invalid \"bodies\" in \"turtlebot\" model, body with " - "name \"base\" already exists"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_I) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_I/world.yaml"); - test_yaml_fail( - "Flatland YAML: Invalid \"joints\" in \"turtlebot\" model, joint with " - "name \"wheel_weld\" already exists"); -} - -/** - * This test tries to load a invalid model yaml file, it should fail - */ -TEST_F(LoadWorldTest, model_invalid_J) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_J/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"points\" must have size <= 8 \\(in model " - "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - ros::init(argc, argv, "load_world_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name load_world_test.cpp + * @brief Testing the load world functionality + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; + +class LoadWorldTest : public ::testing::Test { + protected: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + World *w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + // to test that the world instantiation will fail, and the exception + // message matches the given regex string + void test_yaml_fail(std::string regex_str) { + // do a regex match against error messages + std::cmatch match; + std::regex regex(regex_str); + + try { + w = World::MakeWorld(world_yaml.string()); + ADD_FAILURE() << "Expected an exception, but none were raised"; + } catch (const Exception &e) { + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception &e) { + ADD_FAILURE() << "Expected flatland_server::Exception, another exception " + "was caught instead: " + << e.what(); + } + } + + bool float_cmp(double n1, double n2) { + bool ret = fabs(n1 - n2) < 1e-5; + return ret; + } + + // return the id if found, -1 otherwise + int does_edge_exist(const b2Segment &edge, + const std::vector> &edges) { + for (unsigned int i = 0; i < edges.size(); i++) { + auto e = edges[i]; + if ((float_cmp(edge.point1.x, e.first.x) && + float_cmp(edge.point1.y, e.first.y) && + float_cmp(edge.point2.x, e.second.x) && + float_cmp(edge.point2.y, e.second.y)) || + + (float_cmp(edge.point1.x, e.second.x) && + float_cmp(edge.point1.y, e.second.y) && + float_cmp(edge.point2.x, e.first.x) && + float_cmp(edge.point2.y, e.first.y))) { + return i; + } + } + + return -1; + } + + // checks if one list of edges completely matches the content + // of the other list + bool do_edges_exactly_match( + const std::vector &edges1, + const std::vector> &edges2) { + std::vector> edges_cpy = edges2; + for (unsigned int i = 0; i < edges1.size(); i++) { + auto e = edges1[i]; + int ret_idx = does_edge_exist(e, edges_cpy); + + if (ret_idx < 0) { + return false; + } + + edges_cpy.erase(edges_cpy.begin() + ret_idx); + } + + if (edges_cpy.size() == 0) { + return true; + } else { + return false; + } + } + + bool ColorEq(const Color &c1, const Color &c2) { + if (c1 != c2) { + printf("Color Actual:[%f,%f,%f,%f] != Expected:[%f,%f,%f,%f]\n", c1.r, + c1.g, c1.b, c1.a, c2.r, c2.g, c2.b, c2.a); + return false; + } + return true; + } + + bool BodyEq(Body *body, const std::string name, b2BodyType type, + const std::array &pose, + const std::array &color, double linear_damping, + double angular_damping) { + b2BodyId bid = body->physics_body_; + b2Vec2 t = b2Body_GetPosition(bid); + double a = b2Rot_GetAngle(b2Body_GetRotation(bid)); + + if (name != body->name_) { + printf("Name Actual:%s != Expected:%s\n", body->name_.c_str(), + name.c_str()); + return false; + } + + if (type != b2Body_GetType(bid)) { + printf("Body type Actual:%d != Expected:%d\n", + b2Body_GetType(bid), type); + return false; + } + + if (!float_cmp(t.x, pose[0]) || !float_cmp(t.y, pose[1]) || + !float_cmp(a, pose[2])) { + printf("Pose Actual:[%f,%f,%f] != Expected:[%f,%f,%f]\n", t.x, t.y, a, + pose[0], pose[1], pose[2]); + return false; + } + + if (!ColorEq(body->color_, Color(color))) { + return false; + } + + if (!float_cmp(linear_damping, b2Body_GetLinearDamping(bid))) { + printf("Linear Damping Actual:%f != Expected:%f\n", + b2Body_GetLinearDamping(bid), linear_damping); + return false; + } + + if (!float_cmp(angular_damping, b2Body_GetAngularDamping(bid))) { + printf("Angular Damping Actual %f != Expected:%f\n", + b2Body_GetAngularDamping(bid), angular_damping); + return false; + } + + return true; + } + + bool CircleEq(b2ShapeId sid, double x, double y, double r) { + if (b2Shape_GetType(sid) != b2_circleShape) { + printf("Shape is not of type b2_circleShape, Actual=%d\n", + b2Shape_GetType(sid)); + return false; + } + + b2Circle s = b2Shape_GetCircle(sid); + + if (!float_cmp(r, s.radius) || !float_cmp(x, s.center.x) || + !float_cmp(y, s.center.y)) { + printf("Actual:[x=%f,y=%f,r=%f] != Expected:[%f,%f,%f] \n", s.center.x, + s.center.y, s.radius, x, y, r); + return false; + } + return true; + } + + bool PolygonEq(b2ShapeId sid, std::vector> points) { + if (b2Shape_GetType(sid) != b2_polygonShape) { + printf("Shape is not of type b2_polygonShape, Actual=%d\n", + b2Shape_GetType(sid)); + return false; + } + + b2Polygon s = b2Shape_GetPolygon(sid); + int cnt = s.count; + if (cnt != (int)points.size()) { + printf("Number of points Actual:%d != Expected:%lu\n", cnt, + points.size()); + return false; + } + + auto pts = points; + + for (int i = 0; i < cnt; i++) { + const b2Vec2 p = s.vertices[i]; + + bool found_match = false; + int j; + for (j = 0; j < (int)pts.size(); j++) { + if (float_cmp(p.x, pts[j].first) && float_cmp(p.y, pts[j].second)) { + found_match = true; + break; + } + } + + if (!found_match) { + printf("Actual: ["); + for (int k = 0; k < cnt; k++) { + printf("[%f,%f],", s.vertices[k].x, s.vertices[k].y); + } + printf("] != Expected: ["); + for (int k = 0; k < (int)points.size(); k++) { + printf("[%f,%f],", points[k].first, points[k].second); + } + printf("]\n"); + return false; + } + + pts.erase(pts.begin() + j); + } + + return pts.empty(); + } + + std::vector GetBodyShapes(Body *body) { + b2BodyId bid = body->physics_body_; + int count = b2Body_GetShapeCount(bid); + std::vector shapes(count); + b2Body_GetShapes(bid, shapes.data(), count); + // reverse to match original fixture-list order (box2d v2 returned in + // reverse creation order) + std::reverse(shapes.begin(), shapes.end()); + return shapes; + } + + bool FixtureEq(b2ShapeId sid, bool is_sensor, int group_index, + uint16_t category_bits, uint16_t mask_bits, double density, + double friction, double restitution) { + if (b2Shape_IsSensor(sid) != is_sensor) { + printf("is_sensor Actual:%d != Expected:%d\n", b2Shape_IsSensor(sid), + is_sensor); + return false; + } + + b2Filter filter = b2Shape_GetFilter(sid); + + if (filter.groupIndex != group_index) { + printf("group_index Actual:%d != Expected:%d\n", filter.groupIndex, + group_index); + return false; + } + + if (filter.categoryBits != category_bits) { + printf("category_bits Actual:0x%X != Expected:0x%X\n", + filter.categoryBits, category_bits); + return false; + } + + if (filter.maskBits != mask_bits) { + printf("mask_bits Actual:0x%X != Expected:0x%X\n", filter.maskBits, + mask_bits); + return false; + } + + if (!float_cmp(b2Shape_GetDensity(sid), density)) { + printf("density Actual:%f != Expected:%f\n", b2Shape_GetDensity(sid), + density); + return false; + } + + if (!float_cmp(b2Shape_GetFriction(sid), friction)) { + printf("friction Actual:%f != Expected:%f\n", b2Shape_GetFriction(sid), + friction); + return false; + } + + if (!float_cmp(b2Shape_GetRestitution(sid), restitution)) { + printf("restitution Actual:%f != Expected:%f\n", + b2Shape_GetRestitution(sid), restitution); + return false; + } + + return true; + } + + bool JointEq(Joint *joint, const std::string name, + const std::array &color, Body *body_A, + const std::array &anchor_A, Body *body_B, + const std::array &anchor_B, bool collide_connected) { + b2JointId jid = joint->physics_joint_; + + if (name != joint->name_) { + printf("Name Actual:%s != Expected:%s\n", joint->name_.c_str(), + name.c_str()); + return false; + } + + if (!ColorEq(joint->color_, color)) { + return false; + } + + b2BodyId jbodyA = b2Joint_GetBodyA(jid); + b2BodyId jbodyB = b2Joint_GetBodyB(jid); + + if (!B2_ID_EQUALS(jbodyA, body_A->physics_body_)) { + printf("BodyA mismatch\n"); + return false; + } + + if (!B2_ID_EQUALS(jbodyB, body_B->physics_body_)) { + printf("BodyB mismatch\n"); + return false; + } + + // Get local anchors (type-specific) + b2Vec2 local_anchor_A, local_anchor_B; + b2JointType jtype = b2Joint_GetType(jid); + if (jtype == b2_revoluteJoint) { + local_anchor_A = b2RevoluteJoint_GetLocalAnchorA(jid); + local_anchor_B = b2RevoluteJoint_GetLocalAnchorB(jid); + } else { + local_anchor_A = b2WeldJoint_GetLocalAnchorA(jid); + local_anchor_B = b2WeldJoint_GetLocalAnchorB(jid); + } + + if (!float_cmp(local_anchor_A.x, anchor_A[0]) || + !float_cmp(local_anchor_A.y, anchor_A[1])) { + printf("Anchor A Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_A.x, + local_anchor_A.y, anchor_A[0], anchor_A[1]); + return false; + } + + if (!float_cmp(local_anchor_B.x, anchor_B[0]) || + !float_cmp(local_anchor_B.y, anchor_B[1])) { + printf("Anchor B Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_B.x, + local_anchor_B.y, anchor_B[0], anchor_B[1]); + return false; + } + + if (collide_connected != b2Joint_GetCollideConnected(jid)) { + printf("Collide connected Actual:%d != Expected:%d\n", + b2Joint_GetCollideConnected(jid), collide_connected); + return false; + } + + return true; + } + + bool WeldEq(Joint *joint, double angle, double freq, double damping) { + b2JointId jid = joint->physics_joint_; + + if (b2Joint_GetType(jid) != b2_weldJoint) { + printf("Joint type Actual:%d != Expected:%d(weld joint)\n", + b2Joint_GetType(jid), b2_weldJoint); + return false; + } + + float ref_angle = b2WeldJoint_GetReferenceAngle(jid); + if (!float_cmp(angle, ref_angle)) { + printf("Angle Actual:%f != Expected:%f\n", ref_angle, angle); + return false; + } + + float hertz = b2WeldJoint_GetAngularHertz(jid); + if (!float_cmp(freq, hertz)) { + printf("Frequency Actual:%f != Expected:%f\n", hertz, freq); + return false; + } + + float damp = b2WeldJoint_GetAngularDampingRatio(jid); + if (!float_cmp(damping, damp)) { + printf("Damping Actual:%f != Expected:%f\n", damp, damping); + return false; + } + + return true; + } + + bool RevoluteEq(Joint *joint, bool is_limit_enabled, + const std::array limits) { + b2JointId jid = joint->physics_joint_; + + if (b2Joint_GetType(jid) != b2_revoluteJoint) { + printf("Joint type Actual:%d != Expected:%d(revolute joint)\n", + b2Joint_GetType(jid), b2_revoluteJoint); + return false; + } + + bool limit_enabled = b2RevoluteJoint_IsLimitEnabled(jid); + if (is_limit_enabled != limit_enabled) { + printf("Limits enabled Actual:%d != Expected:%d\n", limit_enabled, + is_limit_enabled); + return false; + } + + if (is_limit_enabled) { + float lower = b2RevoluteJoint_GetLowerLimit(jid); + float upper = b2RevoluteJoint_GetUpperLimit(jid); + if (!float_cmp(limits[0], lower) || !float_cmp(limits[1], upper)) { + printf("Limits Actual:[%f,%f] != Expected:[%f,%f]\n", lower, upper, + limits[0], limits[1]); + return false; + } + } + + return true; + } +}; + +/** + * This test loads the world, layers, models from the given world + * yaml file and checks that all configurations, data, and calculations are + * correct after instantiation + */ +TEST_F(LoadWorldTest, simple_test_A) { + world_yaml = + this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + w = World::MakeWorld(world_yaml.string()); + + EXPECT_EQ(w->physics_velocity_iterations_, 11); + EXPECT_EQ(w->physics_position_iterations_, 12); + + ASSERT_EQ(w->layers_.size(), 4); + + // check that layer 0 settings are loaded correctly + EXPECT_STREQ(w->layers_[0]->name_.c_str(), "2d"); + ASSERT_EQ(w->layers_[0]->names_.size(), 1); + EXPECT_STREQ(w->layers_[0]->names_[0].c_str(), "2d"); + EXPECT_EQ(w->layers_[0]->Type(), Entity::EntityType::LAYER); + EXPECT_TRUE(BodyEq(w->layers_[0]->body_, "2d", b2_staticBody, + {0.05, -0.05, 1.57}, {0, 1, 0, 0.675}, 0, 0)); + EXPECT_EQ(w->cfr_.LookUpLayerId("2d"), 0); + EXPECT_EQ(w->cfr_.GetCategoryBits(w->layers_[0]->names_), 0b1); + + // check that layer 1 settings are loaded correctly + EXPECT_STREQ(w->layers_[1]->name_.c_str(), "3d"); + ASSERT_EQ(w->layers_[1]->names_.size(), 3); + EXPECT_STREQ(w->layers_[1]->names_[0].c_str(), "3d"); + EXPECT_STREQ(w->layers_[1]->names_[1].c_str(), "4d"); + EXPECT_STREQ(w->layers_[1]->names_[2].c_str(), "5d"); + EXPECT_EQ(w->layers_[1]->Type(), Entity::EntityType::LAYER); + EXPECT_TRUE(BodyEq(w->layers_[1]->body_, "3d", b2_staticBody, {0.0, 0.0, 0.0}, + {1, 1, 1, 1}, 0, 0)); + EXPECT_EQ(w->cfr_.LookUpLayerId("3d"), 1); + EXPECT_EQ(w->cfr_.LookUpLayerId("4d"), 2); + EXPECT_EQ(w->cfr_.LookUpLayerId("5d"), 3); + EXPECT_EQ(w->cfr_.GetCategoryBits(w->layers_[1]->names_), 0b1110); + + // check that layer 2 settings are loaded correctly + EXPECT_STREQ(w->layers_[2]->name_.c_str(), "lines"); + ASSERT_EQ(w->layers_[2]->names_.size(), 1); + EXPECT_STREQ(w->layers_[2]->names_[0].c_str(), "lines"); + EXPECT_EQ(w->layers_[2]->Type(), Entity::EntityType::LAYER); + EXPECT_TRUE(BodyEq(w->layers_[2]->body_, "lines", b2_staticBody, + {-1.20, -5, 1.23}, {1, 1, 1, 1}, 0, 0)); + EXPECT_EQ(w->cfr_.LookUpLayerId("lines"), 4); + + // check that bitmap is transformed correctly. This involves flipping the y + // coordinates and apply the resolution. Note that the translation and + // rotation is performed internally by Box2D + std::vector> layer0_expected_edges = { + std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.25, 0.25)), + std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.20, 0.20)), + std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.15, 0.15)), + std::pair(b2Vec2(0.10, 0.10), b2Vec2(0.15, 0.10)), + std::pair(b2Vec2(0.05, 0.05), b2Vec2(0.20, 0.05)), + std::pair(b2Vec2(0.00, 0.00), b2Vec2(0.25, 0.00)), + + std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.00, 0.00)), + std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.05, 0.05)), + std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.10, 0.10)), + std::pair(b2Vec2(0.15, 0.15), b2Vec2(0.15, 0.10)), + std::pair(b2Vec2(0.20, 0.20), b2Vec2(0.20, 0.05)), + std::pair(b2Vec2(0.25, 0.25), b2Vec2(0.25, 0.00))}; + + auto collect_layer_segments = [&](Body* body, uint32_t expected_cat, + uint32_t expected_mask) { + std::vector segs; + int count = b2Body_GetShapeCount(body->physics_body_); + std::vector sids(count); + b2Body_GetShapes(body->physics_body_, sids.data(), count); + for (auto s : sids) { + if (b2Shape_GetType(s) == b2_segmentShape) { + segs.push_back(b2Shape_GetSegment(s)); + b2Filter f = b2Shape_GetFilter(s); + EXPECT_EQ(f.categoryBits, expected_cat); + EXPECT_EQ(f.maskBits, expected_mask); + } + } + return segs; + }; + + std::vector layer0_edges = + collect_layer_segments(w->layers_[0]->body_, 0x1, 0x1); + EXPECT_EQ(layer0_edges.size(), layer0_expected_edges.size()); + EXPECT_TRUE(do_edges_exactly_match(layer0_edges, layer0_expected_edges)); + + // layer[1] has origin of [0, 0, 0], so there should be no transform, just + // apply the inversion of y coordinates and scale by resolution + std::vector> layer1_expected_edges = { + std::pair(b2Vec2(0.0, 7.5), b2Vec2(1.5, 7.5)), + std::pair(b2Vec2(0.0, 7.5), b2Vec2(0.0, 4.5)), + std::pair(b2Vec2(0.0, 4.5), b2Vec2(4.5, 4.5)), + std::pair(b2Vec2(4.5, 4.5), b2Vec2(4.5, 1.5)), + std::pair(b2Vec2(6.0, 3.0), b2Vec2(6.0, 6.0)), + std::pair(b2Vec2(6.0, 6.0), b2Vec2(1.5, 6.0)), + std::pair(b2Vec2(1.5, 7.5), b2Vec2(1.5, 6.0)), + std::pair(b2Vec2(3.0, 3.0), b2Vec2(6.0, 3.0)), + std::pair(b2Vec2(3.0, 0.0), b2Vec2(3.0, 3.0)), + std::pair(b2Vec2(1.5, 1.5), b2Vec2(4.5, 1.5)), + std::pair(b2Vec2(1.5, 0.0), b2Vec2(3.0, 0.0)), + std::pair(b2Vec2(1.5, 1.5), b2Vec2(1.5, 0.0)), + std::pair(b2Vec2(6.0, 1.5), b2Vec2(7.5, 1.5)), + std::pair(b2Vec2(6.0, 1.5), b2Vec2(6.0, 0.0)), + std::pair(b2Vec2(7.5, 1.5), b2Vec2(7.5, 0.0)), + std::pair(b2Vec2(6.0, 0.0), b2Vec2(7.5, 0.0))}; + + std::vector layer1_edges = + collect_layer_segments(w->layers_[1]->body_, 0b1110, 0b1110); + EXPECT_EQ(layer1_edges.size(), layer1_expected_edges.size()); + EXPECT_TRUE(do_edges_exactly_match(layer1_edges, layer1_expected_edges)); + + // check layer[2] data + std::vector> layer2_expected_edges = { + std::pair(b2Vec2(0.1, 0.2), b2Vec2(0.3, 0.4)), + std::pair(b2Vec2(-0.1, -0.2), b2Vec2(-0.3, -0.4)), + std::pair(b2Vec2(0.01, 0.02), b2Vec2(0.03, 0.04))}; + + std::vector layer2_edges = + collect_layer_segments(w->layers_[2]->body_, 0b10000, 0b10000); + EXPECT_EQ(layer2_edges.size(), layer2_expected_edges.size()); + EXPECT_TRUE(do_edges_exactly_match(layer2_edges, layer2_expected_edges)); + + // Check loaded model data + // Check model 0 + Model *m0 = w->models_[0]; + EXPECT_STREQ(m0->name_.c_str(), "turtlebot1"); + EXPECT_STREQ(m0->namespace_.c_str(), ""); + ASSERT_EQ(m0->bodies_.size(), 5); + ASSERT_EQ(m0->joints_.size(), 4); + + // check model 0 body 0 + EXPECT_TRUE(BodyEq(m0->bodies_[0], "base", b2_dynamicBody, {0, 0, 0}, + {1, 1, 0, 0.25}, 0.1, 0.125)); + auto shapes = GetBodyShapes(m0->bodies_[0]); + ASSERT_EQ(shapes.size(), 2); + EXPECT_TRUE(FixtureEq(shapes[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); + EXPECT_TRUE(CircleEq(shapes[0], 0, 0, 1.777)); + EXPECT_TRUE(FixtureEq(shapes[1], false, 0, 0xFFFF, 0xFFFF, 982.24, 0.59, 0.234)); + EXPECT_TRUE( + PolygonEq(shapes[1], {{-0.1, 0.1}, {-0.1, -0.1}, {0.1, -0.1}, {0.1, 0.1}})); + + // check model 0 body 1 + EXPECT_TRUE(BodyEq(m0->bodies_[1], "left_wheel", b2_dynamicBody, {-1, 0, 0}, + {1, 0, 0, 0.25}, 0, 0)); + shapes = GetBodyShapes(m0->bodies_[1]); + ASSERT_EQ(shapes.size(), 1); + EXPECT_TRUE(FixtureEq(shapes[0], true, 0, 0b01, 0b01, 0, 0, 0)); + EXPECT_TRUE(PolygonEq( + shapes[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); + + // check model 0 body 2 + EXPECT_TRUE(BodyEq(m0->bodies_[2], "right_wheel", b2_dynamicBody, {1, 0, 0}, + {0, 1, 0, 0.25}, 0, 0)); + shapes = GetBodyShapes(m0->bodies_[2]); + ASSERT_EQ(shapes.size(), 1); + EXPECT_TRUE(FixtureEq(shapes[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); + EXPECT_TRUE(PolygonEq( + shapes[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); + + // check model 0 body 3 + EXPECT_TRUE(BodyEq(m0->bodies_[3], "tail", b2_dynamicBody, {0, 0, 0.52}, + {0, 0, 0, 0.5}, 0, 0)); + shapes = GetBodyShapes(m0->bodies_[3]); + ASSERT_EQ(shapes.size(), 1); + EXPECT_TRUE(FixtureEq(shapes[0], false, 0, 0b10, 0b10, 0, 0, 0)); + EXPECT_TRUE(PolygonEq(shapes[0], {{-0.2, 0}, {-0.2, -5}, {0.2, -5}, {0.2, 0}})); + + // check model 0 body 4 + EXPECT_TRUE(BodyEq(m0->bodies_[4], "antenna", b2_dynamicBody, {0, 0.5, 0}, + {0.2, 0.4, 0.6, 1}, 0, 0)); + shapes = GetBodyShapes(m0->bodies_[4]); + ASSERT_EQ(shapes.size(), 1); + EXPECT_TRUE(FixtureEq(shapes[0], false, 0, 0b0, 0b0, 0, 0, 0)); + EXPECT_TRUE(CircleEq(shapes[0], 0.01, 0.02, 0.25)); + + // Check loaded joint data + EXPECT_TRUE(JointEq(m0->joints_[0], "left_wheel_weld", {0.1, 0.2, 0.3, 0.4}, + m0->bodies_[0], {-1, 0}, m0->bodies_[1], {0, 0}, true)); + EXPECT_TRUE(WeldEq(m0->joints_[0], 1.57079633, 10, 0.5)); + + EXPECT_TRUE(JointEq(m0->joints_[1], "right_wheel_weld", {1, 1, 1, 0.5}, + m0->bodies_[0], {1, 0}, m0->bodies_[2], {0, 0}, false)); + EXPECT_TRUE(WeldEq(m0->joints_[1], 0, 0, 0)); + + EXPECT_TRUE(JointEq(m0->joints_[2], "tail_revolute", {1, 1, 1, 0.5}, + m0->bodies_[0], {0, 0}, m0->bodies_[3], {0, 0}, false)); + EXPECT_TRUE(RevoluteEq(m0->joints_[2], false, {})); + + EXPECT_TRUE(JointEq(m0->joints_[3], "antenna_revolute", {1, 1, 1, 0.5}, + m0->bodies_[0], {0, 0}, m0->bodies_[4], {0, 0}, true)); + EXPECT_TRUE(RevoluteEq(m0->joints_[3], true, {-0.002, 3.735})); + + // Check model 1 is same yaml file as model 1, simply do a simple sanity check + Model *m1 = w->models_[1]; + EXPECT_STREQ(m1->name_.c_str(), "turtlebot2"); + EXPECT_STREQ(m1->namespace_.c_str(), "robot2"); + ASSERT_EQ(m1->bodies_.size(), 5); + ASSERT_EQ(m1->joints_.size(), 4); + + // check the applied transformation just for the first body + EXPECT_TRUE(BodyEq(m1->bodies_[0], "base", b2_dynamicBody, {3, 4.5, 3.14159}, + {1, 1, 0, 0.25}, 0.1, 0.125)); + + // Check model 2 which is the chair + Model *m2 = w->models_[2]; + EXPECT_STREQ(m2->name_.c_str(), "chair1"); + ASSERT_EQ(m2->bodies_.size(), 1); + ASSERT_EQ(m2->joints_.size(), 0); + + // Check model 2 fixtures + EXPECT_TRUE(BodyEq(m2->bodies_[0], "chair", b2_staticBody, {1.2, 3.5, 2.123}, + {1, 1, 1, 0.5}, 0, 0)); + shapes = GetBodyShapes(m2->bodies_[0]); + ASSERT_EQ(shapes.size(), 2); + EXPECT_TRUE(FixtureEq(shapes[0], false, 0, 0b1100, 0b1100, 0, 0, 0)); + EXPECT_TRUE(CircleEq(shapes[0], 0, 0, 1)); + + EXPECT_TRUE(FixtureEq(shapes[1], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); + EXPECT_TRUE(CircleEq(shapes[1], 0, 0, 0.2)); + + // Check model 3 which is the chair + Model *m3 = w->models_[3]; + EXPECT_STREQ(m3->name_.c_str(), "person1"); + ASSERT_EQ(m3->bodies_.size(), 1); + ASSERT_EQ(m3->joints_.size(), 0); + + // check the body only + EXPECT_TRUE(BodyEq(m3->bodies_[0], "body", b2_kinematicBody, {0, 1, 2}, + {0, 0.75, 0.75, 0.25}, 0, 0)); +} + +/** + * This test tries to loads a non-existent world yaml file. It should throw + * an exception + */ +TEST_F(LoadWorldTest, wrong_world_path) { + world_yaml = + this_file_dir / fs::path("load_world_tests/random_path/world.yaml"); + test_yaml_fail( + "Flatland YAML: File does not exist, " + "path=\".*/load_world_tests/random_path/world.yaml\".*"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_A) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_A/world.yaml"); + test_yaml_fail("Flatland YAML: Entry \"properties\" does not exist"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_B) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_B/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"color\" must have size of exactly 4 \\(in " + "\"layers\" index=0\\)"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_C) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_C/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " + "\\{\"random_param_1\", \"random_param_2\", \"random_param_3\"\\} \\(in " + "\"layers\"\\)"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_D) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_D/world.yaml"); + test_yaml_fail("Flatland YAML: Layer with name \"layer\" already exists"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_E) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_E/world.yaml"); + test_yaml_fail("Flatland YAML: Model with name \"turtlebot\" already exists"); +} + +/** + * This test tries to loads a invalid world yaml file. It should throw + * an exception. + */ +TEST_F(LoadWorldTest, world_invalid_F) { + world_yaml = + this_file_dir / fs::path("load_world_tests/world_invalid_F/world.yaml"); + test_yaml_fail( + "Flatland YAML: Unable to add 3 additional layer\\(s\\) \\{layer_15, " + "layer_16, layer_17\\}, current layers count is 14, max allowed is 16"); +} + +/** + * This test tries to loads valid world yaml file which in turn tries to + * load a invalid map yaml file. It should throw an exception. + */ +TEST_F(LoadWorldTest, map_invalid_A) { + world_yaml = + this_file_dir / fs::path("load_world_tests/map_invalid_A/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"origin\" does not exist \\(in layer " + "\"2d\"\\)"); +} + +/** + * This test tries to loads valid world yaml file which in turn load a map yaml + * file which then inturn tries to load a non-exists map image file. It should + * throw an exception + */ +TEST_F(LoadWorldTest, map_invalid_B) { + world_yaml = + this_file_dir / fs::path("load_world_tests/map_invalid_B/world.yaml"); + test_yaml_fail("Flatland YAML: Failed to load \".*\\.png\" in layer \"2d\""); +} + +/** + * This test tries to load a invalid map configuration, an exception should be + * thrown + */ +TEST_F(LoadWorldTest, map_invalid_C) { + world_yaml = + this_file_dir / fs::path("load_world_tests/map_invalid_C/world.yaml"); + test_yaml_fail( + "Flatland File: Failed to load \".*/map_invalid_C/random_file.dat\""); +} + +/** + * This test tries to load a invalid map configuration, an exception should be + * thrown + */ +TEST_F(LoadWorldTest, map_invalid_D) { + world_yaml = + this_file_dir / fs::path("load_world_tests/map_invalid_D/world.yaml"); + test_yaml_fail( + "Flatland File: Failed to read line segment from line 6, in file " + "\"map_lines.dat\""); +} + +/** + * This test tries to load a invalid map configuration, an exception should be + * thrown + */ +TEST_F(LoadWorldTest, map_invalid_E) { + world_yaml = + this_file_dir / fs::path("load_world_tests/map_invalid_E/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"scale\" does not exist \\(in layer \"lines\"\\)"); +} + +/**` + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_A) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_A/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"bodies\" must be a list \\(in model " + "\"turtlebot\"\\)"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_B) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_B/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"points\" must have size >= 3 \\(in model " + "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_C) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_C/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"anchor\" must have size of exactly 2 \\(in model " + "\"turtlebot\" joint \"right_wheel_weld\" \"bodies\" index=1\\)"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_D) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_D/world.yaml"); + test_yaml_fail( + "Flatland YAML: Cannot find body with name \"left_wheel_123\" in model " + "\"turtlebot\" joint \"left_wheel_weld\" \"bodies\" index=1"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_E) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_E/world.yaml"); + test_yaml_fail( + "Flatland YAML: Invalid footprint \"layers\" in model \"turtlebot\" body " + "\"left_wheel\" \"footprints\" index=0, \\{random_layer\\} layer\\(s\\) " + "does not exist"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_F) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_F/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " + "\\{\"random_paramter\"\\} \\(in model \"turtlebot\" body \"base\" " + "\"footprints\"\\)"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_G) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_G/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry body \"base\" contains unrecognized entry\\(s\\) " + "\\{\"random_paramter\"\\} \\(in model \"turtlebot\"\\)"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_H) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_H/world.yaml"); + test_yaml_fail( + "Flatland YAML: Invalid \"bodies\" in \"turtlebot\" model, body with " + "name \"base\" already exists"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_I) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_I/world.yaml"); + test_yaml_fail( + "Flatland YAML: Invalid \"joints\" in \"turtlebot\" model, joint with " + "name \"wheel_weld\" already exists"); +} + +/** + * This test tries to load a invalid model yaml file, it should fail + */ +TEST_F(LoadWorldTest, model_invalid_J) { + world_yaml = + this_file_dir / fs::path("load_world_tests/model_invalid_J/world.yaml"); + test_yaml_fail( + "Flatland YAML: Entry \"points\" must have size <= 8 \\(in model " + "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + ros::init(argc, argv, "load_world_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/load_world_tests/map_invalid_A/world.yaml b/flatland_server/test/load_world_tests/map_invalid_A/world.yaml index 19e736f8..128285a9 100644 --- a/flatland_server/test/load_world_tests/map_invalid_A/world.yaml +++ b/flatland_server/test/load_world_tests/map_invalid_A/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/map_invalid_A/world_plugins.yaml b/flatland_server/test/load_world_tests/map_invalid_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/map_invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/map_invalid_B/world.yaml b/flatland_server/test/load_world_tests/map_invalid_B/world.yaml index 19e736f8..128285a9 100644 --- a/flatland_server/test/load_world_tests/map_invalid_B/world.yaml +++ b/flatland_server/test/load_world_tests/map_invalid_B/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/map_invalid_B/world_plugins.yaml b/flatland_server/test/load_world_tests/map_invalid_B/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/map_invalid_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/map_invalid_C/world.yaml b/flatland_server/test/load_world_tests/map_invalid_C/world.yaml index a288af85..f3a07fcd 100644 --- a/flatland_server/test/load_world_tests/map_invalid_C/world.yaml +++ b/flatland_server/test/load_world_tests/map_invalid_C/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/map_invalid_C/world_plugins.yaml b/flatland_server/test/load_world_tests/map_invalid_C/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/map_invalid_C/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/map_invalid_D/world.yaml b/flatland_server/test/load_world_tests/map_invalid_D/world.yaml index a288af85..f3a07fcd 100644 --- a/flatland_server/test/load_world_tests/map_invalid_D/world.yaml +++ b/flatland_server/test/load_world_tests/map_invalid_D/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/map_invalid_D/world_plugins.yaml b/flatland_server/test/load_world_tests/map_invalid_D/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/map_invalid_D/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/map_invalid_E/world.yaml b/flatland_server/test/load_world_tests/map_invalid_E/world.yaml index a288af85..f3a07fcd 100644 --- a/flatland_server/test/load_world_tests/map_invalid_E/world.yaml +++ b/flatland_server/test/load_world_tests/map_invalid_E/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/map_invalid_E/world_plugins.yaml b/flatland_server/test/load_world_tests/map_invalid_E/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/map_invalid_E/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_A/world.yaml b/flatland_server/test/load_world_tests/model_invalid_A/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_A/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_A/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_A/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_A/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_B/world.yaml b/flatland_server/test/load_world_tests/model_invalid_B/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_B/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_B/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_B/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_B/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_C/world.yaml b/flatland_server/test/load_world_tests/model_invalid_C/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_C/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_C/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_C/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_C/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_C/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_D/world.yaml b/flatland_server/test/load_world_tests/model_invalid_D/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_D/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_D/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_D/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_D/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_D/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_E/world.yaml b/flatland_server/test/load_world_tests/model_invalid_E/world.yaml index f0661abb..0bc13e69 100644 --- a/flatland_server/test/load_world_tests/model_invalid_E/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_E/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/model_invalid_E/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_E/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_E/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_F/world.yaml b/flatland_server/test/load_world_tests/model_invalid_F/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_F/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_F/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_F/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_F/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_F/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_G/world.yaml b/flatland_server/test/load_world_tests/model_invalid_G/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_G/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_G/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_G/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_G/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_G/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_H/world.yaml b/flatland_server/test/load_world_tests/model_invalid_H/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_H/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_H/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_H/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_H/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_H/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_I/world.yaml b/flatland_server/test/load_world_tests/model_invalid_I/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_I/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_I/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_I/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_I/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_I/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/model_invalid_J/world.yaml b/flatland_server/test/load_world_tests/model_invalid_J/world.yaml index 93ce8661..9691e3f7 100644 --- a/flatland_server/test/load_world_tests/model_invalid_J/world.yaml +++ b/flatland_server/test/load_world_tests/model_invalid_J/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: [] models: - name: turtlebot diff --git a/flatland_server/test/load_world_tests/model_invalid_J/world_plugins.yaml b/flatland_server/test/load_world_tests/model_invalid_J/world_plugins.yaml new file mode 100644 index 00000000..45b360c6 --- /dev/null +++ b/flatland_server/test/load_world_tests/model_invalid_J/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} \ No newline at end of file diff --git a/flatland_server/test/load_world_tests/simple_test_A/world.yaml b/flatland_server/test/load_world_tests/simple_test_A/world.yaml index c096505f..030cfca7 100644 --- a/flatland_server/test/load_world_tests/simple_test_A/world.yaml +++ b/flatland_server/test/load_world_tests/simple_test_A/world.yaml @@ -1,6 +1,3 @@ -properties: - velocity_iterations: 11 - position_iterations: 12 layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/simple_test_A/world_plugins.yaml b/flatland_server/test/load_world_tests/simple_test_A/world_plugins.yaml new file mode 100644 index 00000000..c3245623 --- /dev/null +++ b/flatland_server/test/load_world_tests/simple_test_A/world_plugins.yaml @@ -0,0 +1,3 @@ +properties: + velocity_iterations: 11 + position_iterations: 12 diff --git a/flatland_server/test/load_world_tests/world_invalid_A/world.yaml b/flatland_server/test/load_world_tests/world_invalid_A/world.yaml index 33a1f05b..128285a9 100644 --- a/flatland_server/test/load_world_tests/world_invalid_A/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_A/world.yaml @@ -1,4 +1,3 @@ -properties_____: {} # invalid layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/world_invalid_A/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_A/world_plugins.yaml new file mode 100644 index 00000000..867e9ab0 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_A/world_plugins.yaml @@ -0,0 +1 @@ +properties_____: {} # invalid diff --git a/flatland_server/test/load_world_tests/world_invalid_B/world.yaml b/flatland_server/test/load_world_tests/world_invalid_B/world.yaml index 5d6fc2d0..e7414cd0 100644 --- a/flatland_server/test/load_world_tests/world_invalid_B/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_B/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/world_invalid_B/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_B/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_B/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/load_world_tests/world_invalid_C/world.yaml b/flatland_server/test/load_world_tests/world_invalid_C/world.yaml index 4f43beb1..913bb894 100644 --- a/flatland_server/test/load_world_tests/world_invalid_C/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_C/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/load_world_tests/world_invalid_C/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_C/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_C/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/load_world_tests/world_invalid_D/world.yaml b/flatland_server/test/load_world_tests/world_invalid_D/world.yaml index f3532883..5cb98362 100644 --- a/flatland_server/test/load_world_tests/world_invalid_D/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_D/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "layer" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/world_invalid_D/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_D/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_D/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/load_world_tests/world_invalid_E/world.yaml b/flatland_server/test/load_world_tests/world_invalid_E/world.yaml index 1ab43b07..2cf47831 100644 --- a/flatland_server/test/load_world_tests/world_invalid_E/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_E/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../simple_test_A/map.yaml" diff --git a/flatland_server/test/load_world_tests/world_invalid_E/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_E/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_E/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/load_world_tests/world_invalid_F/world.yaml b/flatland_server/test/load_world_tests/world_invalid_F/world.yaml index 3e019b0e..33d1079c 100644 --- a/flatland_server/test/load_world_tests/world_invalid_F/world.yaml +++ b/flatland_server/test/load_world_tests/world_invalid_F/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - {name: "layer_1", map: "../simple_test_A/map.yaml"} - {name: ["layer_2", "layer_3", "layer_4"], map: "../simple_test_A/map.yaml"} diff --git a/flatland_server/test/load_world_tests/world_invalid_F/world_plugins.yaml b/flatland_server/test/load_world_tests/world_invalid_F/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/load_world_tests/world_invalid_F/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/model_test.cpp b/flatland_server/test/model_test.cpp index f39cdf90..1772252b 100644 --- a/flatland_server/test/model_test.cpp +++ b/flatland_server/test/model_test.cpp @@ -1,92 +1,92 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_test.cpp - * @brief Test model methods and functionality - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/model.h" -#include -#include -#include -#include "flatland_server/collision_filter_registry.h" - -// Test the NameSpaceTF method -TEST(TestSuite, testNameSpaceTF) { - flatland_server::Model has_ns(nullptr, nullptr, std::string("foo"), - std::string("has_ns")); - // namespace "foo" onto tf "bar" => foo_bar - EXPECT_EQ(has_ns.NameSpaceTF("bar"), "foo_bar"); - // namespace "foo" onto tf "/bar" => bar - EXPECT_EQ(has_ns.NameSpaceTF("/bar"), "bar"); - - flatland_server::Model no_ns(nullptr, nullptr, std::string(""), - std::string("no_ns")); - // namespace "" onto tf "bar" => bar - EXPECT_EQ(no_ns.NameSpaceTF("bar"), "bar"); - // namespace "" onto tf "/bar" => bar - EXPECT_EQ(no_ns.NameSpaceTF("/bar"), "bar"); -} - -// Test the NameSpaceTopic method -TEST(TestSuite, testNameSpaceTopic) { - flatland_server::Model has_ns(nullptr, nullptr, std::string("foo"), - std::string("has_ns")); - // namespace "foo" onto tf "bar" => foo_bar - EXPECT_EQ(has_ns.NameSpaceTopic("bar"), "foo/bar"); - // namespace "foo" onto tf "/bar" => bar - EXPECT_EQ(has_ns.NameSpaceTopic("/bar"), "bar"); - - flatland_server::Model no_ns(nullptr, nullptr, std::string(""), - std::string("no_ns")); - // namespace "" onto tf "bar" => bar - EXPECT_EQ(no_ns.NameSpaceTopic("bar"), "bar"); - // namespace "" onto tf "/bar" => bar - EXPECT_EQ(no_ns.NameSpaceTopic("/bar"), "bar"); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - ros::init(argc, argv, "model_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_test.cpp + * @brief Test model methods and functionality + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/model.h" +#include +#include +#include +#include "flatland_server/collision_filter_registry.h" + +// Test the NameSpaceTF method +TEST(TestSuite, testNameSpaceTF) { + flatland_server::Model has_ns(nullptr, nullptr, std::string("foo"), + std::string("has_ns")); + // namespace "foo" onto tf "bar" => foo_bar + EXPECT_EQ(has_ns.NameSpaceTF("bar"), "foo_bar"); + // namespace "foo" onto tf "/bar" => bar + EXPECT_EQ(has_ns.NameSpaceTF("/bar"), "bar"); + + flatland_server::Model no_ns(nullptr, nullptr, std::string(""), + std::string("no_ns")); + // namespace "" onto tf "bar" => bar + EXPECT_EQ(no_ns.NameSpaceTF("bar"), "bar"); + // namespace "" onto tf "/bar" => bar + EXPECT_EQ(no_ns.NameSpaceTF("/bar"), "bar"); +} + +// Test the NameSpaceTopic method +TEST(TestSuite, testNameSpaceTopic) { + flatland_server::Model has_ns(nullptr, nullptr, std::string("foo"), + std::string("has_ns")); + // namespace "foo" onto tf "bar" => foo_bar + EXPECT_EQ(has_ns.NameSpaceTopic("bar"), "foo/bar"); + // namespace "foo" onto tf "/bar" => bar + EXPECT_EQ(has_ns.NameSpaceTopic("/bar"), "bar"); + + flatland_server::Model no_ns(nullptr, nullptr, std::string(""), + std::string("no_ns")); + // namespace "" onto tf "bar" => bar + EXPECT_EQ(no_ns.NameSpaceTopic("bar"), "bar"); + // namespace "" onto tf "/bar" => bar + EXPECT_EQ(no_ns.NameSpaceTopic("/bar"), "bar"); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + ros::init(argc, argv, "model_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/null.cpp b/flatland_server/test/null.cpp index f3d90818..d9d6b800 100644 --- a/flatland_server/test/null.cpp +++ b/flatland_server/test/null.cpp @@ -1,59 +1,59 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name null.cpp - * @brief Sanity check / example test file - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -// Declare a test -TEST(TestSuite, testA) { EXPECT_EQ(1, 1); } - -// Declare another test -TEST(TestSuite, testB) { EXPECT_TRUE(true); } - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name null.cpp + * @brief Sanity check / example test file + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +// Declare a test +TEST(TestSuite, testA) { EXPECT_EQ(1, 1); } + +// Declare another test +TEST(TestSuite, testB) { EXPECT_TRUE(true); } + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/plugin_manager_test.cpp b/flatland_server/test/plugin_manager_test.cpp index 728da15f..7394753b 100644 --- a/flatland_server/test/plugin_manager_test.cpp +++ b/flatland_server/test/plugin_manager_test.cpp @@ -1,404 +1,381 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name plugin_manager_test.cpp - * @brief Testing plugin manager functionalities - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; - -class TestModelPlugin : public ModelPlugin { - public: - // variables used for testing - double timestep_before; - double timestep_after; - Entity *entity; - b2Fixture *fixture_A; - b2Fixture *fixture_B; - - std::map function_called; - - TestModelPlugin() { ClearTestingVariables(); } - - void ClearTestingVariables() { - entity = nullptr; - fixture_A = nullptr; - fixture_B = nullptr; - - function_called["OnInitialize"] = false; - function_called["BeforePhysicsStep"] = false; - function_called["AfterPhysicsStep"] = false; - function_called["BeginContact"] = false; - function_called["EndContact"] = false; - function_called["PreSolve"] = false; - function_called["PostSolve"] = false; - } - - void OnInitialize(const YAML::Node &config) override { - function_called["OnInitialize"] = true; - } - - void BeforePhysicsStep(const Timekeeper &timekeeper) override { - function_called["BeforePhysicsStep"] = true; - } - - void AfterPhysicsStep(const Timekeeper &timekeeper) override { - function_called["AfterPhysicsStep"] = true; - } - - void BeginContact(b2Contact *contact) override { - function_called["BeginContact"] = true; - FilterContact(contact, entity, fixture_A, fixture_B); - } - - void EndContact(b2Contact *contact) override { - function_called["EndContact"] = true; - FilterContact(contact, entity, fixture_A, fixture_B); - } - - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold) override { - function_called["PreSolve"] = true; - FilterContact(contact, entity, fixture_A, fixture_B); - } - - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) override { - function_called["PostSolve"] = true; - FilterContact(contact, entity, fixture_A, fixture_B); - } -}; - -class PluginManagerTest : public ::testing::Test { - protected: - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - Timekeeper timekeeper; - World *w; - - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - w = nullptr; - } - - void TearDown() override { - if (w != nullptr) { - delete w; - } - } - - PluginManagerTest() { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - } - - bool fltcmp(double n1, double n2) { - bool ret = fabs(n1 - n2) < 1e-7; - return ret; - } - - // checks if tow maps have the same keys - bool key_compare(std::map const &lhs, - std::map const &rhs) { - auto pred = [](decltype(*lhs.begin()) a, decltype(a) b) { - return a.first == b.first; - }; - - return lhs.size() == rhs.size() && - std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred); - } - - // Check the true/false if the function is called - bool FunctionCallEq(TestModelPlugin *p, - std::map function_called) { - if (!key_compare(p->function_called, function_called)) { - printf("Two maps does not have the same keys (set of function)\n"); - return false; - } - - for (const auto &func : p->function_called) { - if (func.second != function_called[func.first]) { - printf("%s is %s, expected to be %s\n", func.first.c_str(), - func.second ? "called" : "not called", - function_called[func.first] ? "called" : "not called"); - return false; - } - } - return true; - } -}; - -/** - * This test moves bodies around and test if all the correct functions are - * called with the expected inputs - */ -TEST_F(PluginManagerTest, collision_test) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/collision_test/world.yaml"); - timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); - Layer *l = w->layers_[0]; - Model *m0 = w->models_[0]; - Model *m1 = w->models_[1]; - Body *b0 = m0->bodies_[0]; - Body *b1 = m1->bodies_[0]; - PluginManager *pm = &w->plugin_manager_; - boost::shared_ptr shared_p(new TestModelPlugin()); - shared_p->Initialize("TestModelPlugin", "test_model_plugin", m0, - YAML::Node()); - pm->model_plugins_.push_back(shared_p); - TestModelPlugin *p = shared_p.get(); - - // step the world with everything at zero velocity, this should make sure - // the collision callbacks gets called - w->Update(timekeeper); - w->Update(timekeeper); - - // model 0 is placed right on top of a layer edge at the begining. Thus, - // begin contact should trigger, as well as before and after physics step. - // Note that pre and post solve are never called because the fixtures are - // set as sensors - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", true}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", false}, - {"PostSolve", false}})); - EXPECT_EQ(p->entity, l); - EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); - EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); - p->ClearTestingVariables(); - - // move the body 2m to the left over two 1s timesteps, this should remove any - // contacts between the body and the layer - b0->physics_body_->SetLinearVelocity(b2Vec2(-1, 0)); - // takes two steps for Box2D to genreate collision events, not sure why - w->Update(timekeeper); - w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", false}, - {"EndContact", true}, - {"PreSolve", false}, - {"PostSolve", false}})); - EXPECT_EQ(p->entity, l); - EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); - EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); - p->ClearTestingVariables(); - - // move the body 1m down over 2 timesteps, this should place model 0 in - // contact with model 1 - b0->physics_body_->SetLinearVelocity(b2Vec2(0, 0)); - b1->physics_body_->SetLinearVelocity(b2Vec2(0, -0.5)); - w->Update(timekeeper); - w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", false}, - {"PostSolve", false}})); - EXPECT_EQ(p->entity, m1); - EXPECT_EQ(p->fixture_B, b1->physics_body_->GetFixtureList()); - EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); - p->ClearTestingVariables(); - - // move the body 2m down over 2 timesteps, this should clear any contacts for - // model 0 - b0->physics_body_->SetLinearVelocity(b2Vec2(0, 0)); - b1->physics_body_->SetLinearVelocity(b2Vec2(0, -1)); - w->Update(timekeeper); - w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", false}, - {"EndContact", true}, - {"PreSolve", false}, - {"PostSolve", false}})); - EXPECT_EQ(p->entity, m1); - EXPECT_EQ(p->fixture_B, b1->physics_body_->GetFixtureList()); - EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); - p->ClearTestingVariables(); - - // Now we set model 0 fixture as not a sensor, this should trigger pre and - // post solves in the contact listener in subsequent tests - b0->physics_body_->GetFixtureList()->SetSensor(false); - - // now teleport the body for model 0 to (0, 0) which is right on top of a - // layer edge, set zero velocity and step, this will cause the body - // to begin contact with the layer, but you can't be sure if end contact - // will be called - b0->physics_body_->SetLinearVelocity(b2Vec2(0, 0)); - b0->physics_body_->SetTransform(b2Vec2(0, 0), 0); - w->Update(timekeeper); - w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", true}, - {"PostSolve", true}})); - EXPECT_EQ(p->entity, l); - EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); - EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); - p->ClearTestingVariables(); - - // w->DebugVisualize(); - // DebugVisualization::Get().Publish(); - // ros::spin(); -} - -TEST_F(PluginManagerTest, load_dummy_test) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - - w = World::MakeWorld(world_yaml.string()); - - ModelPlugin *p = w->plugin_manager_.model_plugins_[0].get(); - - EXPECT_STREQ(p->GetType().c_str(), "DummyModelPlugin"); - EXPECT_STREQ(p->GetName().c_str(), "dummy_test_plugin"); -} - -TEST_F(PluginManagerTest, plugin_throws_exception) { - world_yaml = - this_file_dir / - fs::path("plugin_manager_tests/plugin_throws_exception/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException &e) { - // do a regex match against error message - std::string regex_str = - ".*dummy_param_float must be dummy_test_123456, instead it was " - "\"wrong_message\".*"; - std::cmatch match; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -TEST_F(PluginManagerTest, nonexistent_plugin) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/nonexistent_plugin/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException &e) { - std::cmatch match; - std::string regex_str = - ".*RandomPlugin with base class type flatland_server::ModelPlugin does " - "not exist.*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { - ADD_FAILURE() << "Was expecting a PluginException, another exception was " - "caught instead: " - << e.what(); - } -} - -TEST_F(PluginManagerTest, invalid_plugin_yaml) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/invalid_plugin_yaml/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - FAIL() << "Expected an exception, but none were raised"; - } catch (const YAMLException &e) { - EXPECT_STREQ( - "Flatland YAML: Entry \"name\" does not exist (in model \"turtlebot1\" " - "\"plugins\" index=0)", - e.what()); - } catch (const std::exception &e) { - ADD_FAILURE() << "Was expecting a YAMLException, another exception was " - "caught instead: " - << e.what(); - } -} - -TEST_F(PluginManagerTest, duplicate_plugin) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/duplicate_plugin/world.yaml"); - - try { - w = World::MakeWorld(world_yaml.string()); - FAIL() << "Expected an exception, but none were raised"; - } catch (const YAMLException &e) { - EXPECT_STREQ( - "Flatland YAML: Invalid \"plugins\" in \"turtlebot1\" model, plugin " - "with name \"dummy_test_plugin\" already exists", - e.what()); - } catch (const std::exception &e) { - ADD_FAILURE() << "Was expecting a YAMLException, another exception was " - "caught instead: " - << e.what(); - } -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - ros::init(argc, argv, "plugin_manager_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name plugin_manager_test.cpp + * @brief Testing plugin manager functionalities + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; + +class TestModelPlugin : public ModelPlugin { + public: + // variables used for testing + double timestep_before; + double timestep_after; + Entity *entity; + b2ShapeId shape_A; + b2ShapeId shape_B; + + std::map function_called; + + TestModelPlugin() { ClearTestingVariables(); } + + void ClearTestingVariables() { + entity = nullptr; + shape_A = b2_nullShapeId; + shape_B = b2_nullShapeId; + + function_called["OnInitialize"] = false; + function_called["BeforePhysicsStep"] = false; + function_called["AfterPhysicsStep"] = false; + function_called["BeginContact"] = false; + function_called["EndContact"] = false; + } + + void OnInitialize(const YAML::Node &config) override { + function_called["OnInitialize"] = true; + } + + void BeforePhysicsStep(const Timekeeper &timekeeper) override { + function_called["BeforePhysicsStep"] = true; + } + + void AfterPhysicsStep(const Timekeeper &timekeeper) override { + function_called["AfterPhysicsStep"] = true; + } + + void BeginContact(b2ShapeId shapeA, b2ShapeId shapeB) override { + function_called["BeginContact"] = true; + b2BodyId bodyA, bodyB; + FilterContact(shapeA, shapeB, entity, bodyA, bodyB); + shape_A = shapeA; + shape_B = shapeB; + } + + void EndContact(b2ShapeId shapeA, b2ShapeId shapeB) override { + function_called["EndContact"] = true; + b2BodyId bodyA, bodyB; + FilterContact(shapeA, shapeB, entity, bodyA, bodyB); + shape_A = shapeA; + shape_B = shapeB; + } +}; + +class PluginManagerTest : public ::testing::Test { + protected: + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + Timekeeper timekeeper; + World *w; + + void SetUp() override { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + w = nullptr; + } + + void TearDown() override { + if (w != nullptr) { + delete w; + } + } + + PluginManagerTest() { + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + } + + bool fltcmp(double n1, double n2) { + bool ret = fabs(n1 - n2) < 1e-7; + return ret; + } + + // checks if tow maps have the same keys + bool key_compare(std::map const &lhs, + std::map const &rhs) { + auto pred = [](decltype(*lhs.begin()) a, decltype(a) b) { + return a.first == b.first; + }; + + return lhs.size() == rhs.size() && + std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred); + } + + // Check the true/false if the function is called + bool FunctionCallEq(TestModelPlugin *p, + std::map function_called) { + if (!key_compare(p->function_called, function_called)) { + printf("Two maps does not have the same keys (set of function)\n"); + return false; + } + + for (const auto &func : p->function_called) { + if (func.second != function_called[func.first]) { + printf("%s is %s, expected to be %s\n", func.first.c_str(), + func.second ? "called" : "not called", + function_called[func.first] ? "called" : "not called"); + return false; + } + } + return true; + } +}; + +/** + * This test moves bodies around and test if all the correct functions are + * called with the expected inputs + */ +TEST_F(PluginManagerTest, collision_test) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/collision_test/world.yaml"); + timekeeper.SetMaxStepSize(1.0); + w = World::MakeWorld(world_yaml.string()); + Layer *l = w->layers_[0]; + Model *m0 = w->models_[0]; + Model *m1 = w->models_[1]; + Body *b0 = m0->bodies_[0]; + Body *b1 = m1->bodies_[0]; + PluginManager *pm = &w->plugin_manager_; + boost::shared_ptr shared_p(new TestModelPlugin()); + shared_p->Initialize("TestModelPlugin", "test_model_plugin", m0, + YAML::Node()); + pm->model_plugins_.push_back(shared_p); + TestModelPlugin *p = shared_p.get(); + + // step the world with everything at zero velocity, this should make sure + // the collision callbacks gets called + w->Update(timekeeper); + w->Update(timekeeper); + + // model 0 is placed right on top of a layer edge at the beginning. Thus, + // begin contact should trigger, as well as before and after physics step. + EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", true}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}})); + EXPECT_EQ(p->entity, l); + // shape_A should belong to b0's body + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_A), b0->physics_body_)); + // shape_B should be a segment shape (layer edge) + EXPECT_EQ(b2Shape_GetType(p->shape_B), b2_segmentShape); + p->ClearTestingVariables(); + + // move the body 2m to the left over two 1s timesteps, this should remove any + // contacts between the body and the layer + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{-1, 0}); + // takes two steps for Box2D to generate collision events + w->Update(timekeeper); + w->Update(timekeeper); + EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", false}, + {"EndContact", true}})); + EXPECT_EQ(p->entity, l); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_A), b0->physics_body_)); + EXPECT_EQ(b2Shape_GetType(p->shape_B), b2_segmentShape); + p->ClearTestingVariables(); + + // move the body 1m down over 2 timesteps, this should place model 0 in + // contact with model 1 + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{0, 0}); + b2Body_SetLinearVelocity(b1->physics_body_, b2Vec2{0, -0.5f}); + w->Update(timekeeper); + w->Update(timekeeper); + EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}})); + EXPECT_EQ(p->entity, m1); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_B), b1->physics_body_)); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_A), b0->physics_body_)); + p->ClearTestingVariables(); + + // move the body 2m down over 2 timesteps, this should clear any contacts for + // model 0 + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{0, 0}); + b2Body_SetLinearVelocity(b1->physics_body_, b2Vec2{0, -1.0f}); + w->Update(timekeeper); + w->Update(timekeeper); + EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", false}, + {"EndContact", true}})); + EXPECT_EQ(p->entity, m1); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_B), b1->physics_body_)); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_A), b0->physics_body_)); + p->ClearTestingVariables(); + + // now teleport b0 back to (0, 0) on top of the layer edge + b2Body_SetLinearVelocity(b0->physics_body_, b2Vec2{0, 0}); + b2Body_SetTransform(b0->physics_body_, b2Vec2{0, 0}, b2MakeRot(0)); + w->Update(timekeeper); + w->Update(timekeeper); + EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}})); + EXPECT_EQ(p->entity, l); + EXPECT_TRUE(B2_ID_EQUALS(b2Shape_GetBody(p->shape_A), b0->physics_body_)); + EXPECT_EQ(b2Shape_GetType(p->shape_B), b2_segmentShape); + p->ClearTestingVariables(); + + // w->DebugVisualize(); + // DebugVisualization::Get().Publish(); + // ros::spin(); +} + +TEST_F(PluginManagerTest, load_dummy_test) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); + + w = World::MakeWorld(world_yaml.string()); + + ModelPlugin *p = w->plugin_manager_.model_plugins_[0].get(); + + EXPECT_STREQ(p->GetType().c_str(), "DummyModelPlugin"); + EXPECT_STREQ(p->GetName().c_str(), "dummy_test_plugin"); +} + +TEST_F(PluginManagerTest, plugin_throws_exception) { + world_yaml = + this_file_dir / + fs::path("plugin_manager_tests/plugin_throws_exception/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException &e) { + // do a regex match against error message + std::string regex_str = + ".*dummy_param_float must be dummy_test_123456, instead it was " + "\"wrong_message\".*"; + std::cmatch match; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception &e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +TEST_F(PluginManagerTest, nonexistent_plugin) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/nonexistent_plugin/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + FAIL() << "Expected an exception, but none were raised"; + } catch (const PluginException &e) { + std::cmatch match; + std::string regex_str = + ".*RandomPlugin with base class type flatland_server::ModelPlugin does " + "not exist.*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(e.what(), match, regex)) + << "Exception Message '" + std::string(e.what()) + "'" + + " did not match against regex '" + regex_str + "'"; + } catch (const std::exception &e) { + ADD_FAILURE() << "Was expecting a PluginException, another exception was " + "caught instead: " + << e.what(); + } +} + +TEST_F(PluginManagerTest, invalid_plugin_yaml) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/invalid_plugin_yaml/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + FAIL() << "Expected an exception, but none were raised"; + } catch (const YAMLException &e) { + EXPECT_STREQ( + "Flatland YAML: Entry \"name\" does not exist (in model \"turtlebot1\" " + "\"plugins\" index=0)", + e.what()); + } catch (const std::exception &e) { + ADD_FAILURE() << "Was expecting a YAMLException, another exception was " + "caught instead: " + << e.what(); + } +} + +TEST_F(PluginManagerTest, duplicate_plugin) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/duplicate_plugin/world.yaml"); + + try { + w = World::MakeWorld(world_yaml.string()); + FAIL() << "Expected an exception, but none were raised"; + } catch (const YAMLException &e) { + EXPECT_STREQ( + "Flatland YAML: Invalid \"plugins\" in \"turtlebot1\" model, plugin " + "with name \"dummy_test_plugin\" already exists", + e.what()); + } catch (const std::exception &e) { + ADD_FAILURE() << "Was expecting a YAMLException, another exception was " + "caught instead: " + << e.what(); + } +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + ros::init(argc, argv, "plugin_manager_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/plugin_manager_tests/collision_test/world.yaml b/flatland_server/test/plugin_manager_tests/collision_test/world.yaml index d275e929..9ff2dcd6 100644 --- a/flatland_server/test/plugin_manager_tests/collision_test/world.yaml +++ b/flatland_server/test/plugin_manager_tests/collision_test/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../load_dummy_test/map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/collision_test/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/collision_test/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/collision_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/plugin_manager_tests/duplicate_plugin/world.yaml b/flatland_server/test/plugin_manager_tests/duplicate_plugin/world.yaml index d275e929..9ff2dcd6 100644 --- a/flatland_server/test/plugin_manager_tests/duplicate_plugin/world.yaml +++ b/flatland_server/test/plugin_manager_tests/duplicate_plugin/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../load_dummy_test/map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/duplicate_plugin/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/duplicate_plugin/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/duplicate_plugin/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world.yaml b/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world.yaml index d275e929..9ff2dcd6 100644 --- a/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world.yaml +++ b/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../load_dummy_test/map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/invalid_plugin_yaml/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/plugin_manager_tests/load_dummy_test/world.yaml b/flatland_server/test/plugin_manager_tests/load_dummy_test/world.yaml index a7d58227..a1bc075d 100644 --- a/flatland_server/test/plugin_manager_tests/load_dummy_test/world.yaml +++ b/flatland_server/test/plugin_manager_tests/load_dummy_test/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/load_dummy_test/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/load_dummy_test/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/load_dummy_test/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world.yaml b/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world.yaml index d275e929..9ff2dcd6 100644 --- a/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world.yaml +++ b/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../load_dummy_test/map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/nonexistent_plugin/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world.yaml b/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world.yaml index d275e929..9ff2dcd6 100644 --- a/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world.yaml +++ b/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world.yaml @@ -1,4 +1,3 @@ -properties: {} layers: - name: "2d" map: "../load_dummy_test/map.yaml" diff --git a/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world_plugins.yaml b/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world_plugins.yaml new file mode 100644 index 00000000..7b474df9 --- /dev/null +++ b/flatland_server/test/plugin_manager_tests/plugin_throws_exception/world_plugins.yaml @@ -0,0 +1 @@ +properties: {} diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 923756ca..5150d98f 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -1,288 +1,286 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name service_manager_test.cpp - * @brief Testing service manager functionalities - * @author Chunshang Li - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; - -class ServiceManagerTest : public ::testing::Test { - protected: - SimulationManager* sim_man; - boost::filesystem::path this_file_dir; - boost::filesystem::path world_yaml; - boost::filesystem::path robot_yaml; - Timekeeper timekeeper; - ros::NodeHandle nh; - ros::ServiceClient client; - std::thread simulation_thread; - - void SetUp() override { - sim_man = nullptr; - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - timekeeper.SetMaxStepSize(1.0); - } - - void TearDown() override { - StopSimulationThread(); - if (sim_man) delete sim_man; - } - - void StartSimulationThread() { - sim_man = - new SimulationManager(world_yaml.string(), 1000, 1 / 1000.0, false, 0); - simulation_thread = std::thread(&ServiceManagerTest::SimulationThread, - dynamic_cast(this)); - } - - void StopSimulationThread() { - sim_man->Shutdown(); - simulation_thread.join(); - } - - void SimulationThread() { sim_man->Main(); } -}; - -/** - * Testing service for loading a model which should succeed - */ -TEST_F(ServiceManagerTest, spawn_valid_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - - robot_yaml = this_file_dir / - fs::path("load_world_tests/simple_test_A/person.model.yaml"); - - flatland_msgs::SpawnModel srv; - - srv.request.name = "service_manager_test_robot"; - srv.request.ns = "robot123"; - srv.request.yaml_path = robot_yaml.string(); - srv.request.pose.x = 101.1; - srv.request.pose.y = 102.1; - srv.request.pose.theta = 0.23; - - client = nh.serviceClient("spawn_model"); - - // Threading is required since client.call blocks executing until return - StartSimulationThread(); - - ros::service::waitForService("spawn_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_TRUE(srv.response.success); - ASSERT_STREQ("", srv.response.message.c_str()); - - World* w = sim_man->world_; - ASSERT_EQ(5, w->models_.size()); - EXPECT_STREQ("service_manager_test_robot", w->models_[4]->name_.c_str()); - EXPECT_STREQ("robot123", w->models_[4]->namespace_.c_str()); - EXPECT_FLOAT_EQ(101.1, - w->models_[4]->bodies_[0]->physics_body_->GetPosition().x); - EXPECT_FLOAT_EQ(102.1, - w->models_[4]->bodies_[0]->physics_body_->GetPosition().y); - EXPECT_FLOAT_EQ(0.23, w->models_[4]->bodies_[0]->physics_body_->GetAngle()); - EXPECT_EQ(1, w->models_[4]->bodies_.size()); -} - -/** - * Testing service for loading a model which should fail - */ -TEST_F(ServiceManagerTest, spawn_invalid_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - - robot_yaml = this_file_dir / fs::path("random_path/turtlebot.model.yaml"); - - flatland_msgs::SpawnModel srv; - - srv.request.name = "service_manager_test_robot"; - srv.request.yaml_path = robot_yaml.string(); - srv.request.pose.x = 1; - srv.request.pose.y = 2; - srv.request.pose.theta = 3; - - client = nh.serviceClient("spawn_model"); - - StartSimulationThread(); - - ros::service::waitForService("spawn_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_FALSE(srv.response.success); - - std::cmatch match; - std::string regex_str = - "Flatland YAML: File does not exist, " - "path=\".*/random_path/turtlebot.model.yaml\".*"; - std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(srv.response.message.c_str(), match, regex)) - << "Error Message '" + srv.response.message + "'" + - " did not match against regex '" + regex_str + "'"; -} - -/** - * Testing service for moving a valid model - */ -TEST_F(ServiceManagerTest, move_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - - flatland_msgs::MoveModel srv; - srv.request.name = "turtlebot1"; - srv.request.pose.x = 5.5; - srv.request.pose.y = 9.9; - srv.request.pose.theta = 0.77; - - client = nh.serviceClient("move_model"); - - StartSimulationThread(); - - ros::service::waitForService("move_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_TRUE(srv.response.success); - - World* w = sim_man->world_; - EXPECT_NEAR(5.5, w->models_[0]->bodies_[0]->physics_body_->GetPosition().x, - 1e-2); - EXPECT_NEAR(9.9, w->models_[0]->bodies_[0]->physics_body_->GetPosition().y, - 1e-2); - EXPECT_NEAR(0.77, w->models_[0]->bodies_[0]->physics_body_->GetAngle(), 1e-2); -} - -/** - * Testing service for moving a nonexistent model - */ -TEST_F(ServiceManagerTest, move_nonexistent_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - - flatland_msgs::MoveModel srv; - srv.request.name = "not_a_robot"; - srv.request.pose.x = 4; - srv.request.pose.y = 5; - srv.request.pose.theta = 0; - - client = nh.serviceClient("move_model"); - - StartSimulationThread(); - - ros::service::waitForService("move_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_FALSE(srv.response.success); - EXPECT_STREQ( - "Flatland World: failed to move model, model with name " - "\"not_a_robot\" does not exist", - srv.response.message.c_str()); -} - -/** - * Testing service for deleting a model - */ -TEST_F(ServiceManagerTest, delete_model) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - - flatland_msgs::DeleteModel srv; - srv.request.name = "turtlebot1"; - - client = nh.serviceClient("delete_model"); - - StartSimulationThread(); - - ros::service::waitForService("delete_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_TRUE(srv.response.success); - World* w = sim_man->world_; - // after deleting a mode, there should be one less model, and one less plugin - ASSERT_EQ(w->models_.size(), 0); - ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0); - int count = std::count_if(w->models_.begin(), w->models_.end(), - [](Model* m) { return m->name_ == "turtlebot1"; }); - ASSERT_EQ(count, 0); -} - -/** - * Testing service for deleting a model that does not exist, should fail - */ -TEST_F(ServiceManagerTest, delete_nonexistent_model) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - - flatland_msgs::DeleteModel srv; - srv.request.name = "random_model"; - - client = nh.serviceClient("delete_model"); - - StartSimulationThread(); - - ros::service::waitForService("delete_model", 1000); - ASSERT_TRUE(client.call(srv)); - - ASSERT_FALSE(srv.response.success); - EXPECT_STREQ( - "Flatland World: failed to delete model, model with name " - "\"random_model\" does not exist", - srv.response.message.c_str()); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char** argv) { - ros::init(argc, argv, "service_manager_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name service_manager_test.cpp + * @brief Testing service manager functionalities + * @author Chunshang Li + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; + +class ServiceManagerTest : public ::testing::Test { + protected: + SimulationManager* sim_man; + boost::filesystem::path this_file_dir; + boost::filesystem::path world_yaml; + boost::filesystem::path robot_yaml; + Timekeeper timekeeper; + ros::NodeHandle nh; + ros::ServiceClient client; + std::thread simulation_thread; + + void SetUp() override { + sim_man = nullptr; + this_file_dir = boost::filesystem::path(__FILE__).parent_path(); + timekeeper.SetMaxStepSize(1.0); + } + + void TearDown() override { + StopSimulationThread(); + if (sim_man) delete sim_man; + } + + void StartSimulationThread() { + sim_man = + new SimulationManager(world_yaml.string(), 1000, 1 / 1000.0, false, 0); + simulation_thread = std::thread(&ServiceManagerTest::SimulationThread, + dynamic_cast(this)); + } + + void StopSimulationThread() { + sim_man->Shutdown(); + simulation_thread.join(); + } + + void SimulationThread() { sim_man->Main(); } +}; + +/** + * Testing service for loading a model which should succeed + */ +TEST_F(ServiceManagerTest, spawn_valid_model) { + world_yaml = + this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + + robot_yaml = this_file_dir / + fs::path("load_world_tests/simple_test_A/person.model.yaml"); + + flatland_msgs::SpawnModel srv; + + srv.request.name = "service_manager_test_robot"; + srv.request.ns = "robot123"; + srv.request.yaml_path = robot_yaml.string(); + srv.request.pose.x = 101.1; + srv.request.pose.y = 102.1; + srv.request.pose.theta = 0.23; + + client = nh.serviceClient("spawn_model"); + + // Threading is required since client.call blocks executing until return + StartSimulationThread(); + + ros::service::waitForService("spawn_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_TRUE(srv.response.success); + ASSERT_STREQ("", srv.response.message.c_str()); + + World* w = sim_man->world_; + ASSERT_EQ(5, w->models_.size()); + EXPECT_STREQ("service_manager_test_robot", w->models_[4]->name_.c_str()); + EXPECT_STREQ("robot123", w->models_[4]->namespace_.c_str()); + EXPECT_FLOAT_EQ(101.1, + b2Body_GetPosition(w->models_[4]->bodies_[0]->physics_body_).x); + EXPECT_FLOAT_EQ(102.1, + b2Body_GetPosition(w->models_[4]->bodies_[0]->physics_body_).y); + EXPECT_FLOAT_EQ(0.23, b2Rot_GetAngle(b2Body_GetRotation(w->models_[4]->bodies_[0]->physics_body_))); + EXPECT_EQ(1, w->models_[4]->bodies_.size()); +} + +/** + * Testing service for loading a model which should fail + */ +TEST_F(ServiceManagerTest, spawn_invalid_model) { + world_yaml = + this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + + robot_yaml = this_file_dir / fs::path("random_path/turtlebot.model.yaml"); + + flatland_msgs::SpawnModel srv; + + srv.request.name = "service_manager_test_robot"; + srv.request.yaml_path = robot_yaml.string(); + srv.request.pose.x = 1; + srv.request.pose.y = 2; + srv.request.pose.theta = 3; + + client = nh.serviceClient("spawn_model"); + + StartSimulationThread(); + + ros::service::waitForService("spawn_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_FALSE(srv.response.success); + + std::cmatch match; + std::string regex_str = + "Flatland YAML: File does not exist, " + "path=\".*/random_path/turtlebot.model.yaml\".*"; + std::regex regex(regex_str); + EXPECT_TRUE(std::regex_match(srv.response.message.c_str(), match, regex)) + << "Error Message '" + srv.response.message + "'" + + " did not match against regex '" + regex_str + "'"; +} + +/** + * Testing service for moving a valid model + */ +TEST_F(ServiceManagerTest, move_model) { + world_yaml = + this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + + flatland_msgs::MoveModel srv; + srv.request.name = "turtlebot1"; + srv.request.pose.x = 5.5; + srv.request.pose.y = 9.9; + srv.request.pose.theta = 0.77; + + client = nh.serviceClient("move_model"); + + StartSimulationThread(); + + ros::service::waitForService("move_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_TRUE(srv.response.success); + + World* w = sim_man->world_; + EXPECT_NEAR(5.5, b2Body_GetPosition(w->models_[0]->bodies_[0]->physics_body_).x, 1e-2); + EXPECT_NEAR(9.9, b2Body_GetPosition(w->models_[0]->bodies_[0]->physics_body_).y, 1e-2); + EXPECT_NEAR(0.77, b2Rot_GetAngle(b2Body_GetRotation(w->models_[0]->bodies_[0]->physics_body_)), 1e-2); +} + +/** + * Testing service for moving a nonexistent model + */ +TEST_F(ServiceManagerTest, move_nonexistent_model) { + world_yaml = + this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + + flatland_msgs::MoveModel srv; + srv.request.name = "not_a_robot"; + srv.request.pose.x = 4; + srv.request.pose.y = 5; + srv.request.pose.theta = 0; + + client = nh.serviceClient("move_model"); + + StartSimulationThread(); + + ros::service::waitForService("move_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_FALSE(srv.response.success); + EXPECT_STREQ( + "Flatland World: failed to move model, model with name " + "\"not_a_robot\" does not exist", + srv.response.message.c_str()); +} + +/** + * Testing service for deleting a model + */ +TEST_F(ServiceManagerTest, delete_model) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); + + flatland_msgs::DeleteModel srv; + srv.request.name = "turtlebot1"; + + client = nh.serviceClient("delete_model"); + + StartSimulationThread(); + + ros::service::waitForService("delete_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_TRUE(srv.response.success); + World* w = sim_man->world_; + // after deleting a mode, there should be one less model, and one less plugin + ASSERT_EQ(w->models_.size(), 0); + ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0); + int count = std::count_if(w->models_.begin(), w->models_.end(), + [](Model* m) { return m->name_ == "turtlebot1"; }); + ASSERT_EQ(count, 0); +} + +/** + * Testing service for deleting a model that does not exist, should fail + */ +TEST_F(ServiceManagerTest, delete_nonexistent_model) { + world_yaml = this_file_dir / + fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); + + flatland_msgs::DeleteModel srv; + srv.request.name = "random_model"; + + client = nh.serviceClient("delete_model"); + + StartSimulationThread(); + + ros::service::waitForService("delete_model", 1000); + ASSERT_TRUE(client.call(srv)); + + ASSERT_FALSE(srv.response.success); + EXPECT_STREQ( + "Flatland World: failed to delete model, model with name " + "\"random_model\" does not exist", + srv.response.message.c_str()); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + ros::init(argc, argv, "service_manager_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/test/yaml_preprocessor/yaml/include.child.yaml b/flatland_server/test/yaml_preprocessor/yaml/include.child.yaml new file mode 100644 index 00000000..f9277602 --- /dev/null +++ b/flatland_server/test/yaml_preprocessor/yaml/include.child.yaml @@ -0,0 +1,3 @@ +foo: bar +spam: 2 +eggs: $include include.string.yaml diff --git a/flatland_server/test/yaml_preprocessor/yaml/include.parent.out.yaml b/flatland_server/test/yaml_preprocessor/yaml/include.parent.out.yaml new file mode 100644 index 00000000..250a0e26 --- /dev/null +++ b/flatland_server/test/yaml_preprocessor/yaml/include.parent.out.yaml @@ -0,0 +1,15 @@ +a: this is a +b: 52 +c: this is a technically a YAML string. +d: contents of absolute file +e: + foo: bar + spam: 2 + eggs: this is a technically a YAML string. +f: + - parent first element + - first element + - second element + - third element + - parent last element +g: [] diff --git a/flatland_server/test/yaml_preprocessor/yaml/include.parent.yaml b/flatland_server/test/yaml_preprocessor/yaml/include.parent.yaml new file mode 100644 index 00000000..35acaab4 --- /dev/null +++ b/flatland_server/test/yaml_preprocessor/yaml/include.parent.yaml @@ -0,0 +1,10 @@ +a: this is a +b: 52 +c: $include include.string.yaml +d: $include /tmp/9aaafd40-2083-49d9-a300-9d01f94d6671.yaml +e: $include include.child.yaml +f: + - parent first element + - $[include] include.sequence.yaml + - parent last element +g: [] # Test: make sure empty lists are not modified \ No newline at end of file diff --git a/flatland_server/test/yaml_preprocessor/yaml/include.sequence.yaml b/flatland_server/test/yaml_preprocessor/yaml/include.sequence.yaml new file mode 100644 index 00000000..56cd4273 --- /dev/null +++ b/flatland_server/test/yaml_preprocessor/yaml/include.sequence.yaml @@ -0,0 +1,5 @@ +first element +--- +second element +--- +third element diff --git a/flatland_server/test/yaml_preprocessor/yaml/include.string.yaml b/flatland_server/test/yaml_preprocessor/yaml/include.string.yaml new file mode 100644 index 00000000..396ca4e0 --- /dev/null +++ b/flatland_server/test/yaml_preprocessor/yaml/include.string.yaml @@ -0,0 +1 @@ +this is a technically a YAML string. diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp index 50bfd619..81328d20 100644 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp +++ b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp @@ -1,135 +1,135 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name null.cpp - * @brief Sanity check / example test file - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_server/yaml_preprocessor.h" -#include -#include -#include - -namespace fs = boost::filesystem; -using namespace flatland_server; - -void compareNodes(const char *p1, YAML::Node &a, YAML::Node &b) { - try { - EXPECT_STREQ(b[p1].as().c_str(), - a[p1].as().c_str()) - << "at " << p1; - } catch (...) { - ADD_FAILURE() << "Failure to compare " << p1; - } -} - -void compareNodes(const char *p1, int p2, YAML::Node &a, YAML::Node &b) { - try { - EXPECT_STREQ(b[p1][p2].as().c_str(), - a[p1][p2].as().c_str()) - << "at " << p1 << ":" << p2; - } catch (...) { - ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; - } -} - -void compareNodes(const char *p1, const char *p2, YAML::Node &a, - YAML::Node &b) { - try { - EXPECT_STREQ(b[p1][p2].as().c_str(), - a[p1][p2].as().c_str()) - << "at " << p1 << ":" << p2; - } catch (...) { - ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; - } -} - -// Test the bodyToMarkers method on a polygon shape -TEST(YamlPreprocTest, testEvalStrings) { - boost::filesystem::path cwd = fs::path(__FILE__).parent_path(); - - YAML::Node in = YamlPreprocessor::LoadParse( - (cwd / fs::path("/yaml/eval.strings.yaml")).string()); - - YAML::Node out = YamlPreprocessor::LoadParse( - (cwd / fs::path("/yaml/eval.strings.out.yaml")).string()); - - // check that the two strings match - compareNodes("foo", in, out); - compareNodes("bar", in, out); - compareNodes("baz", in, out); - compareNodes("bash", in, out); - - compareNodes("boop", "bal", in, out); - - compareNodes("foop", 0, in, out); - compareNodes("foop", 1, in, out); - compareNodes("foop", 2, in, out); - - compareNodes("bom", in, out); - - compareNodes("testEnv", "env1", in, out); - compareNodes("testEnv", "env2", in, out); - compareNodes("testEnv", "env3", in, out); - compareNodes("testEnv", "env4", in, out); - compareNodes("testEnv", "env5", in, out); - compareNodes("testEnv", "env6", in, out); - compareNodes("testEnv", "env7", in, out); - compareNodes("testEnv", "env8", in, out); - - compareNodes("testParam", "param1", in, out); - compareNodes("testParam", "param2", in, out); - compareNodes("testParam", "param3", in, out); - compareNodes("testParam", "param4", in, out); - compareNodes("testParam", "param5", in, out); - compareNodes("testParam", "param6", in, out); - compareNodes("testParam", "param7", in, out); - compareNodes("testParam", "param8", in, out); - compareNodes("testParam", "param9", in, out); -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) { - ros::init(argc, argv, "yaml_preprocessor_test"); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name null.cpp + * @brief Sanity check / example test file + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_server/yaml_preprocessor.h" +#include +#include +#include + +namespace fs = boost::filesystem; +using namespace flatland_server; + +void compareNodes(const char *p1, YAML::Node &a, YAML::Node &b) { + try { + EXPECT_STREQ(b[p1].as().c_str(), + a[p1].as().c_str()) + << "at " << p1; + } catch (...) { + ADD_FAILURE() << "Failure to compare " << p1; + } +} + +void compareNodes(const char *p1, int p2, YAML::Node &a, YAML::Node &b) { + try { + EXPECT_STREQ(b[p1][p2].as().c_str(), + a[p1][p2].as().c_str()) + << "at " << p1 << ":" << p2; + } catch (...) { + ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; + } +} + +void compareNodes(const char *p1, const char *p2, YAML::Node &a, + YAML::Node &b) { + try { + EXPECT_STREQ(b[p1][p2].as().c_str(), + a[p1][p2].as().c_str()) + << "at " << p1 << ":" << p2; + } catch (...) { + ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; + } +} + +// Test the bodyToMarkers method on a polygon shape +TEST(YamlPreprocTest, testEvalStrings) { + boost::filesystem::path cwd = fs::path(__FILE__).parent_path(); + + YAML::Node in = YamlPreprocessor::LoadParse( + (cwd / fs::path("/yaml/eval.strings.yaml")).string()); + + YAML::Node out = YamlPreprocessor::LoadParse( + (cwd / fs::path("/yaml/eval.strings.out.yaml")).string()); + + // check that the two strings match + compareNodes("foo", in, out); + compareNodes("bar", in, out); + compareNodes("baz", in, out); + compareNodes("bash", in, out); + + compareNodes("boop", "bal", in, out); + + compareNodes("foop", 0, in, out); + compareNodes("foop", 1, in, out); + compareNodes("foop", 2, in, out); + + compareNodes("bom", in, out); + + compareNodes("testEnv", "env1", in, out); + compareNodes("testEnv", "env2", in, out); + compareNodes("testEnv", "env3", in, out); + compareNodes("testEnv", "env4", in, out); + compareNodes("testEnv", "env5", in, out); + compareNodes("testEnv", "env6", in, out); + compareNodes("testEnv", "env7", in, out); + compareNodes("testEnv", "env8", in, out); + + compareNodes("testParam", "param1", in, out); + compareNodes("testParam", "param2", in, out); + compareNodes("testParam", "param3", in, out); + compareNodes("testParam", "param4", in, out); + compareNodes("testParam", "param5", in, out); + compareNodes("testParam", "param6", in, out); + compareNodes("testParam", "param7", in, out); + compareNodes("testParam", "param8", in, out); + compareNodes("testParam", "param9", in, out); +} + +// Run all the tests that were declared with TEST() +int main(int argc, char **argv) { + ros::init(argc, argv, "yaml_preprocessor_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp b/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp index 201d5160..60a125a5 100644 --- a/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp +++ b/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp @@ -1016,13 +1016,13 @@ struct b2WorldRayCastWrapper b2RayCastCallback* callback; }; -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const +void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction/*=1.0*/) const { b2WorldRayCastWrapper wrapper; wrapper.broadPhase = &m_contactManager.m_broadPhase; wrapper.callback = callback; b2RayCastInput input; - input.maxFraction = 1.0f; + input.maxFraction = maxFraction; input.p1 = point1; input.p2 = point2; m_contactManager.m_broadPhase.RayCast(&wrapper, input); diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.h b/flatland_server/thirdparty/Box2D/Dynamics/b2World.h index fa0ba0b8..460b9310 100644 --- a/flatland_server/thirdparty/Box2D/Dynamics/b2World.h +++ b/flatland_server/thirdparty/Box2D/Dynamics/b2World.h @@ -119,7 +119,7 @@ class b2World /// @param callback a user implemented callback class. /// @param point1 the ray starting point /// @param point2 the ray ending point - void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; + void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2, float maxFraction=1.0f) const; /// Get the world body list. With the returned body, use b2Body::GetNext to get /// the next body in the world list. A nullptr body indicates the end of the list. diff --git a/flatland_viz/CHANGELOG.rst b/flatland_viz/CHANGELOG.rst new file mode 100644 index 00000000..c0751aed --- /dev/null +++ b/flatland_viz/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package flatland_viz +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.4.1 (2023-11-24) +------------------ +* CMake version required bump to fix ros build farm warning + +1.4.0 (2023-11-22) +------------------ +* Version Bump + +1.3.3 (2023-02-06) +------------------ +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne + +* Merge pull request `#95 `_ from avidbots/per-package-licenses + Per package licenses +* Contributors: Joseph Duchesne diff --git a/flatland_viz/CMakeLists.txt b/flatland_viz/CMakeLists.txt index fa238af0..5c50788c 100644 --- a/flatland_viz/CMakeLists.txt +++ b/flatland_viz/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(flatland_viz) # Get the LSB release name (14.04 or 16.04) string into variable ${LSB_RELEASE_VALUE} @@ -70,6 +70,7 @@ link_directories(${catkin_LIBRARY_DIRS}) ## This setting causes Qt's "MOC" generation to happen automatically. set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTOMOC_COMPILER_PREDEFINES OFF) ## This plugin includes Qt widgets, so we must include Qt. ## We'll use the version that rviz used so they are compatible. diff --git a/flatland_viz/LICENSE b/flatland_viz/LICENSE new file mode 100644 index 00000000..32802b81 --- /dev/null +++ b/flatland_viz/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, Avidbots Corp. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/flatland_viz/include/flatland_viz/flatland_viz.h b/flatland_viz/include/flatland_viz/flatland_viz.h index 84d500fc..10b015a6 100644 --- a/flatland_viz/include/flatland_viz/flatland_viz.h +++ b/flatland_viz/include/flatland_viz/flatland_viz.h @@ -1,158 +1,158 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_viz.h - * @brief Manages the librviz viewport for flatland - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATLAND_VIZ_FLATLAND_VIZ_H -#define FLATLAND_VIZ_FLATLAND_VIZ_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flatland_msgs/DebugTopicList.h" -#include "rviz/config.h" -#include "rviz/panel.h" -#include "rviz/properties/property_tree_widget.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/window_manager_interface.h" - -class QSplashScreen; -class QAction; -class QActionGroup; -class QTimer; -class QDockWidget; -class QLabel; -class QToolButton; - -namespace rviz { -class PanelFactory; -class Tool; -class Display; -class RenderPanel; -class VisualizationManager; -class WidgetGeometryChangeDetector; -} - -class FlatlandWindow; - -class FlatlandViz : public QWidget { - Q_OBJECT public : - /** - * @brief Construct FlatlandViz and subscribe to debug topic list - * - * @param parent The parent widget - */ - FlatlandViz(FlatlandWindow* parent = 0); - - /** - * @brief Recieve a new DebugTopicList msg and add any new displays required - * - * @param msg The DebugTopicList message - */ - void RecieveDebugTopics(const flatland_msgs::DebugTopicList& msg); - - /** - * @brief Destruct - */ - virtual ~FlatlandViz(); - - rviz::VisualizationManager* manager_; - - private: - rviz::RenderPanel* render_panel_; - - rviz::Display* grid_; - rviz::Display* interactive_markers_; - std::map debug_displays_; - ros::Subscriber debug_topic_subscriber_; - rviz::PropertyTreeWidget* tree_widget_; - FlatlandWindow* parent_; - - QMenu* file_menu_; - QMenu* recent_configs_menu_; - QMenu* view_menu_; - QMenu* delete_view_menu_; - QMenu* plugins_menu_; - - QToolBar* toolbar_; - - QActionGroup* toolbar_actions_; - std::map action_to_tool_map_; - std::map tool_to_action_map_; - bool show_choose_new_master_option_; - - QAction* add_tool_action_; - QMenu* remove_tool_menu_; - - /// Indicates if the toolbar should be visible outside of fullscreen mode. - bool toolbar_visible_; - - // protected Q_SLOTS: - void fullScreenChange(bool hidden); - - void setDisplayConfigModified(); - void addTool(rviz::Tool*); - void removeTool(rviz::Tool*); - void refreshTool(rviz::Tool*); - void indicateToolIsCurrent(rviz::Tool*); - void onToolbarActionTriggered(QAction* action); - void onToolbarRemoveTool(QAction* remove_tool_menu_action); - void initToolbars(); - void initMenus(); - void openNewToolDialog(); - void setFullScreen(bool full_screen); -}; - -#endif // FLATLAND_VIZ_FLATLAND_VIZ_H \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_viz.h + * @brief Manages the librviz viewport for flatland + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_VIZ_FLATLAND_VIZ_H +#define FLATLAND_VIZ_FLATLAND_VIZ_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flatland_msgs/DebugTopicList.h" +#include "rviz/config.h" +#include "rviz/panel.h" +#include "rviz/properties/property_tree_widget.h" +#include "rviz/tool.h" +#include "rviz/tool_manager.h" +#include "rviz/window_manager_interface.h" + +class QSplashScreen; +class QAction; +class QActionGroup; +class QTimer; +class QDockWidget; +class QLabel; +class QToolButton; + +namespace rviz { +class PanelFactory; +class Tool; +class Display; +class RenderPanel; +class VisualizationManager; +class WidgetGeometryChangeDetector; +} + +class FlatlandWindow; + +class FlatlandViz : public QWidget { + Q_OBJECT public : + /** + * @brief Construct FlatlandViz and subscribe to debug topic list + * + * @param parent The parent widget + */ + FlatlandViz(FlatlandWindow* parent = 0); + + /** + * @brief Recieve a new DebugTopicList msg and add any new displays required + * + * @param msg The DebugTopicList message + */ + void RecieveDebugTopics(const flatland_msgs::DebugTopicList& msg); + + /** + * @brief Destruct + */ + virtual ~FlatlandViz(); + + rviz::VisualizationManager* manager_; + + private: + rviz::RenderPanel* render_panel_; + + rviz::Display* grid_; + rviz::Display* interactive_markers_; + std::map debug_displays_; + ros::Subscriber debug_topic_subscriber_; + rviz::PropertyTreeWidget* tree_widget_; + FlatlandWindow* parent_; + + QMenu* file_menu_; + QMenu* recent_configs_menu_; + QMenu* view_menu_; + QMenu* delete_view_menu_; + QMenu* plugins_menu_; + + QToolBar* toolbar_; + + QActionGroup* toolbar_actions_; + std::map action_to_tool_map_; + std::map tool_to_action_map_; + bool show_choose_new_master_option_; + + QAction* add_tool_action_; + QMenu* remove_tool_menu_; + + /// Indicates if the toolbar should be visible outside of fullscreen mode. + bool toolbar_visible_; + + // protected Q_SLOTS: + void fullScreenChange(bool hidden); + + void setDisplayConfigModified(); + void addTool(rviz::Tool*); + void removeTool(rviz::Tool*); + void refreshTool(rviz::Tool*); + void indicateToolIsCurrent(rviz::Tool*); + void onToolbarActionTriggered(QAction* action); + void onToolbarRemoveTool(QAction* remove_tool_menu_action); + void initToolbars(); + void initMenus(); + void openNewToolDialog(); + void setFullScreen(bool full_screen); +}; + +#endif // FLATLAND_VIZ_FLATLAND_VIZ_H diff --git a/flatland_viz/include/flatland_viz/flatland_window.h b/flatland_viz/include/flatland_viz/flatland_window.h index 469ad8b6..ba3b7654 100644 --- a/flatland_viz/include/flatland_viz/flatland_window.h +++ b/flatland_viz/include/flatland_viz/flatland_window.h @@ -1,100 +1,100 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_window.cpp - * @brief Main window and toolbars for flatland_viz - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// namespace rviz; - -#include -#include -#include -#include -#include -#include "flatland_viz/flatland_viz.h" - -#include "rviz/display.h" -#include "rviz/display_context.h" -#include "rviz/displays_panel.h" -#include "rviz/env_config.h" -#include "rviz/failed_panel.h" -#include "rviz/help_panel.h" -#include "rviz/load_resource.h" -#include "rviz/loading_dialog.h" -#include "rviz/new_object_dialog.h" -#include "rviz/panel_dock_widget.h" -#include "rviz/panel_factory.h" -#include "rviz/properties/status_property.h" -#include "rviz/render_panel.h" -#include "rviz/screenshot_dialog.h" -#include "rviz/selection/selection_manager.h" -#include "rviz/selection_panel.h" -#include "rviz/splash_screen.h" -#include "rviz/time_panel.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/tool_properties_panel.h" -#include "rviz/view_manager.h" -#include "rviz/views_panel.h" -#include "rviz/visualization_frame.h" -#include "rviz/visualization_manager.h" -#include "rviz/widget_geometry_change_detector.h" -#include "rviz/yaml_config_reader.h" -#include "rviz/yaml_config_writer.h" - -class FlatlandWindow : public QMainWindow { - Q_OBJECT - public: - FlatlandWindow(QWidget* parent = 0); - rviz::VisualizationManager* visualization_manager_; - rviz::RenderPanel* render_panel_; - - rviz::VisualizationManager* getManager(); - - protected Q_SLOTS: - - void openNewToolDialog(); - - private: - FlatlandViz* viz_; -}; +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_window.cpp + * @brief Main window and toolbars for flatland_viz + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +// namespace rviz; + +#include +#include +#include +#include +#include +#include "flatland_viz/flatland_viz.h" + +#include "rviz/display.h" +#include "rviz/display_context.h" +#include "rviz/displays_panel.h" +#include "rviz/env_config.h" +#include "rviz/failed_panel.h" +#include "rviz/help_panel.h" +#include "rviz/load_resource.h" +#include "rviz/loading_dialog.h" +#include "rviz/new_object_dialog.h" +#include "rviz/panel_dock_widget.h" +#include "rviz/panel_factory.h" +#include "rviz/properties/status_property.h" +#include "rviz/render_panel.h" +#include "rviz/screenshot_dialog.h" +#include "rviz/selection/selection_manager.h" +#include "rviz/selection_panel.h" +#include "rviz/splash_screen.h" +#include "rviz/time_panel.h" +#include "rviz/tool.h" +#include "rviz/tool_manager.h" +#include "rviz/tool_properties_panel.h" +#include "rviz/view_manager.h" +#include "rviz/views_panel.h" +#include "rviz/visualization_frame.h" +#include "rviz/visualization_manager.h" +#include "rviz/widget_geometry_change_detector.h" +#include "rviz/yaml_config_reader.h" +#include "rviz/yaml_config_writer.h" + +class FlatlandWindow : public QMainWindow { + Q_OBJECT + public: + FlatlandWindow(QWidget* parent = 0); + rviz::VisualizationManager* visualization_manager_; + rviz::RenderPanel* render_panel_; + + rviz::VisualizationManager* getManager(); + + protected Q_SLOTS: + + void openNewToolDialog(); + + private: + FlatlandViz* viz_; +}; diff --git a/flatland_viz/include/flatland_viz/load_model_dialog.h b/flatland_viz/include/flatland_viz/load_model_dialog.h index 72ec175a..ffc3b1e6 100644 --- a/flatland_viz/include/flatland_viz/load_model_dialog.h +++ b/flatland_viz/include/flatland_viz/load_model_dialog.h @@ -1,136 +1,136 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name load_model_dialog.h - * @brief Spawn dialog - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef LOAD_MODEL_DIALOG_H -#define LOAD_MODEL_DIALOG_H - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -// #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace flatland_viz { -class SpawnModelTool; -} - -class LoadModelDialog : public QDialog { - public: - /** - * @name LoadModelDialog - * @brief Launch load model dialog - * @param parent, parent widget pointer - * @param tool, pointer to this so dialog can call methods - */ - LoadModelDialog(QWidget *parent, flatland_viz::SpawnModelTool *tool); - - private: - /** - * @name ChooseFile - * @brief Launch file selection dialog - */ - QString ChooseFile(); - /** - * @name AddNumberAndUpdateName - * @brief Add numbering to name and show in the name widget - */ - void AddNumberAndUpdateName(); - - static QString path_to_model_file; // full path to model file - static int count; // counter for adding unique number to filename - static bool numbering; // flag to use unique numbering - - flatland_viz::SpawnModelTool *tool_; - QLineEdit *n_edit; // name lineEdit widget - QLabel *p_label; // path label widget - QCheckBox *n_checkbox; // checkbox widget - - public Q_SLOTS: - /** - * @name NumberCheckBoxChanged - * @brief Checkbox was clicked, toggle numbering of names - */ - void NumberCheckBoxChanged(bool value); - /** - * @name CancelButtonClicked - * @brief Cancel button was clicked, dismiss dialog - */ - void CancelButtonClicked(); - /** - * @name OkButtonClicked - * @brief Ok button was clicked, begin placement - */ - void OkButtonClicked(); - /** - * @name PathButtonClicked - * @brief Path button was clicked, launch file selection dialog - */ - void on_PathButtonClicked(); -}; - -#endif // LOAD_MODEL_DIALOG_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name load_model_dialog.h + * @brief Spawn dialog + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef LOAD_MODEL_DIALOG_H +#define LOAD_MODEL_DIALOG_H + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace flatland_viz { +class SpawnModelTool; +} + +class LoadModelDialog : public QDialog { + public: + /** + * @name LoadModelDialog + * @brief Launch load model dialog + * @param parent, parent widget pointer + * @param tool, pointer to this so dialog can call methods + */ + LoadModelDialog(QWidget *parent, flatland_viz::SpawnModelTool *tool); + + private: + /** + * @name ChooseFile + * @brief Launch file selection dialog + */ + QString ChooseFile(); + /** + * @name AddNumberAndUpdateName + * @brief Add numbering to name and show in the name widget + */ + void AddNumberAndUpdateName(); + + static QString path_to_model_file; // full path to model file + static int count; // counter for adding unique number to filename + static bool numbering; // flag to use unique numbering + + flatland_viz::SpawnModelTool *tool_; + QLineEdit *n_edit; // name lineEdit widget + QLabel *p_label; // path label widget + QCheckBox *n_checkbox; // checkbox widget + + public Q_SLOTS: + /** + * @name NumberCheckBoxChanged + * @brief Checkbox was clicked, toggle numbering of names + */ + void NumberCheckBoxChanged(bool value); + /** + * @name CancelButtonClicked + * @brief Cancel button was clicked, dismiss dialog + */ + void CancelButtonClicked(); + /** + * @name OkButtonClicked + * @brief Ok button was clicked, begin placement + */ + void OkButtonClicked(); + /** + * @name PathButtonClicked + * @brief Path button was clicked, launch file selection dialog + */ + void on_PathButtonClicked(); +}; + +#endif // LOAD_MODEL_DIALOG_H diff --git a/flatland_viz/include/flatland_viz/model_dialog.h b/flatland_viz/include/flatland_viz/model_dialog.h index 0c72ae99..a6d73961 100644 --- a/flatland_viz/include/flatland_viz/model_dialog.h +++ b/flatland_viz/include/flatland_viz/model_dialog.h @@ -1,133 +1,133 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_dialog.h - * @brief Main window and toolbars for flatland_viz - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef MODEL_DIALOG_H -#define MODEL_DIALOG_H - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include -#include - -class QCheckBox; -class QLabel; -class QErrorMessage; - -class DialogOptionsWidget; - -namespace fs = boost::filesystem; -using namespace flatland_server; - -class ModelDialog : public QDialog { - Q_OBJECT - - public: - static QColor saved_color_; - - ModelDialog(QWidget* parent = 0); - - private Q_SLOTS: - /** - * @name SetColor - * @brief Callback to pop up a ColorDialog - */ - void SetColor(); - /** - * @name CancelButtonClicked - * @brief Callback to dismiss the model dialog (cancel was clicked) - */ - void CancelButtonClicked(); - /** - * @name OkButtonClicked - * @brief Callback to create the model (ok was clicked) - */ - void OkButtonClicked(); - /** - * @name SelectFile - * @brief Callback to choose model - */ - QString SelectFile(); - /** - * @name SetButtonColor - * @brief Changes a button's color - * @param[in] QColor, color to set button to (incl alpha) - * @param[in] QPushButton, button to set color on - */ - void SetButtonColor(const QColor* c, QPushButton* b); - /** - * @name SpawnModelClient - * @brief Makes a call to spawn model ros service - */ - - void SpawnModelClient(); - - private: - QPushButton* color_button; - QString path_to_model_file; - QLineEdit *x_edit, *y_edit, *a_edit, *n_edit; - - protected: - boost::filesystem::path this_file_dir; - ros::NodeHandle nh; - ros::ServiceClient client; - flatland_msgs::SpawnModel srv; - World* w; -}; - -#endif \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_dialog.h + * @brief Main window and toolbars for flatland_viz + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef MODEL_DIALOG_H +#define MODEL_DIALOG_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include + +class QCheckBox; +class QLabel; +class QErrorMessage; + +class DialogOptionsWidget; + +namespace fs = boost::filesystem; +using namespace flatland_server; + +class ModelDialog : public QDialog { + Q_OBJECT + + public: + static QColor saved_color_; + + ModelDialog(QWidget* parent = 0); + + private Q_SLOTS: + /** + * @name SetColor + * @brief Callback to pop up a ColorDialog + */ + void SetColor(); + /** + * @name CancelButtonClicked + * @brief Callback to dismiss the model dialog (cancel was clicked) + */ + void CancelButtonClicked(); + /** + * @name OkButtonClicked + * @brief Callback to create the model (ok was clicked) + */ + void OkButtonClicked(); + /** + * @name SelectFile + * @brief Callback to choose model + */ + QString SelectFile(); + /** + * @name SetButtonColor + * @brief Changes a button's color + * @param[in] QColor, color to set button to (incl alpha) + * @param[in] QPushButton, button to set color on + */ + void SetButtonColor(const QColor* c, QPushButton* b); + /** + * @name SpawnModelClient + * @brief Makes a call to spawn model ros service + */ + + void SpawnModelClient(); + + private: + QPushButton* color_button; + QString path_to_model_file; + QLineEdit *x_edit, *y_edit, *a_edit, *n_edit; + + protected: + boost::filesystem::path this_file_dir; + ros::NodeHandle nh; + ros::ServiceClient client; + flatland_msgs::SpawnModel srv; + World* w; +}; + +#endif diff --git a/flatland_viz/include/flatland_viz/pause_sim_tool.h b/flatland_viz/include/flatland_viz/pause_sim_tool.h index d95f36e1..6e6ccbe3 100644 --- a/flatland_viz/include/flatland_viz/pause_sim_tool.h +++ b/flatland_viz/include/flatland_viz/pause_sim_tool.h @@ -1,46 +1,46 @@ -#ifndef PAUSE_SIM_TOOL_H -#define PAUSE_SIM_TOOL_H - -#include -#include -#include - -namespace flatland_viz { - -/** - * @name PauseSimTool - * @brief Rviz tool to support pausing and unpausing the - * simulation. - */ -class PauseSimTool : public rviz::Tool { - public: - PauseSimTool(); - ~PauseSimTool(); - - private: - /** - * @name onInitialize - * @brief Initializes tools currently loaded when rviz starts - */ - virtual void onInitialize(); - - /** - virtual void activate(); - * @name activate - * @brief Call the pause toggle service - */ - virtual void activate(); - - /** - * @name deactivate - * @brief Cleanup when tool is removed - */ - virtual void deactivate(); - - ros::NodeHandle nh_; ///< NodeHandle to call the pause toggle service - ros::ServiceClient - pause_service_; ///< ServiceClient that calls the pause toggle service -}; -} - -#endif // PAUSE_SIM_TOOL_H +#ifndef PAUSE_SIM_TOOL_H +#define PAUSE_SIM_TOOL_H + +#include +#include +#include + +namespace flatland_viz { + +/** + * @name PauseSimTool + * @brief Rviz tool to support pausing and unpausing the + * simulation. + */ +class PauseSimTool : public rviz::Tool { + public: + PauseSimTool(); + ~PauseSimTool(); + + private: + /** + * @name onInitialize + * @brief Initializes tools currently loaded when rviz starts + */ + virtual void onInitialize(); + + /** + virtual void activate(); + * @name activate + * @brief Call the pause toggle service + */ + virtual void activate(); + + /** + * @name deactivate + * @brief Cleanup when tool is removed + */ + virtual void deactivate(); + + ros::NodeHandle nh_; ///< NodeHandle to call the pause toggle service + ros::ServiceClient + pause_service_; ///< ServiceClient that calls the pause toggle service +}; +} + +#endif // PAUSE_SIM_TOOL_H diff --git a/flatland_viz/include/flatland_viz/spawn_model_tool.h b/flatland_viz/include/flatland_viz/spawn_model_tool.h index 6766c907..ed80a83d 100644 --- a/flatland_viz/include/flatland_viz/spawn_model_tool.h +++ b/flatland_viz/include/flatland_viz/spawn_model_tool.h @@ -1,171 +1,171 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name spawn_model_tool.h - * @brief Rviz compatible tool for spawning flatland model - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef SPAWN_MODEL_TOOL_H -#define SPAWN_MODEL_TOOL_H - -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include "rviz/ogre_helpers/arrow.h" - -namespace flatland_viz { -/** - * @name SpawnModelTool - * @brief Every tool which can be added to the tool bar is a - * subclass of rviz::Tool. - */ -class SpawnModelTool : public rviz::Tool { - Q_OBJECT - - public: - SpawnModelTool(); - ~SpawnModelTool(); - - /** - * @name BeginPlacement - * @brief Begin the placement phase, model follows cursor, left - * click deposits model and starts rotation phase - */ - void BeginPlacement(); - /** - * @name SavePath - * @brief Called from dialog to save path - */ - void SavePath(QString p); - /** - * @name SaveName - * @brief Called from dialog to save name - */ - void SaveName(QString n); - /** - * @name SpawnModelInFlatland - * @brief Spawns a model using ros service - */ - void SpawnModelInFlatland(); - - private: - /** - * @name onInitialize - * @brief Initializes tools currently loaded when rviz starts - */ - virtual void onInitialize(); - /** - virtual void activate(); - * @name activate - * @brief Launch the model dialog - */ - virtual void activate(); - /** - * @name deactivate - * @brief Cleanup when tool is removed - */ - virtual void deactivate(); - /** - * @name processMouseEvent - * @brief Main loop of tool - * @param event Mouse event - */ - virtual int processMouseEvent(rviz::ViewportMouseEvent &event); - /** - * @name SetMovingModelColor - * @brief Set the color of the moving model - * @param c QColor to set the 3d model - */ - void SetMovingModelColor(QColor c); - /** - * @name LoadPreview - * @brief Load a vector preview of the model - */ - void LoadPreview(); - /** - * @name LoadPolygonFootprint - * @brief Load a vector preview of the model's polygon footprint - * @param footprint The footprint yaml node - * @param pose x,y,theta pose of footprint - */ - void LoadPolygonFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose); - /** - * @name LoadCircleFootprint - * @brief Load a vector preview of the model's circle footprint - * @param footprint The footprint yaml node - * @param pose x,y,theta pose of footprint - */ - void LoadCircleFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose); - - Ogre::Vector3 - intersection; // location cursor intersects ground plane, ie the - // location to create the model - float initial_angle; // the angle to create the model at - Ogre::SceneNode *moving_model_node_; // the node for the 3D object - enum ModelState { m_hidden, m_dragging, m_rotating }; - ModelState model_state; // model state, first hidden, then dragging to - // intersection point, then rotating to desired angle - static QString path_to_model_file_; // full path to model file (yaml) - static QString model_name; // base file name with path and extension removed - - protected: - rviz::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation - ros::NodeHandle nh; // ros service node handle - ros::ServiceClient client; // ros service client - std::vector> lines_list_; -}; - -} // end namespace flatland_viz - -#endif // SPAWN_MODEL_TOOL_H +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name spawn_model_tool.h + * @brief Rviz compatible tool for spawning flatland model + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef SPAWN_MODEL_TOOL_H +#define SPAWN_MODEL_TOOL_H + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include "rviz/ogre_helpers/arrow.h" + +namespace flatland_viz { +/** + * @name SpawnModelTool + * @brief Every tool which can be added to the tool bar is a + * subclass of rviz::Tool. + */ +class SpawnModelTool : public rviz::Tool { + Q_OBJECT + + public: + SpawnModelTool(); + ~SpawnModelTool(); + + /** + * @name BeginPlacement + * @brief Begin the placement phase, model follows cursor, left + * click deposits model and starts rotation phase + */ + void BeginPlacement(); + /** + * @name SavePath + * @brief Called from dialog to save path + */ + void SavePath(QString p); + /** + * @name SaveName + * @brief Called from dialog to save name + */ + void SaveName(QString n); + /** + * @name SpawnModelInFlatland + * @brief Spawns a model using ros service + */ + void SpawnModelInFlatland(); + + private: + /** + * @name onInitialize + * @brief Initializes tools currently loaded when rviz starts + */ + virtual void onInitialize(); + /** + virtual void activate(); + * @name activate + * @brief Launch the model dialog + */ + virtual void activate(); + /** + * @name deactivate + * @brief Cleanup when tool is removed + */ + virtual void deactivate(); + /** + * @name processMouseEvent + * @brief Main loop of tool + * @param event Mouse event + */ + virtual int processMouseEvent(rviz::ViewportMouseEvent &event); + /** + * @name SetMovingModelColor + * @brief Set the color of the moving model + * @param c QColor to set the 3d model + */ + void SetMovingModelColor(QColor c); + /** + * @name LoadPreview + * @brief Load a vector preview of the model + */ + void LoadPreview(); + /** + * @name LoadPolygonFootprint + * @brief Load a vector preview of the model's polygon footprint + * @param footprint The footprint yaml node + * @param pose x,y,theta pose of footprint + */ + void LoadPolygonFootprint(flatland_server::YamlReader &footprint, + const flatland_server::Pose pose); + /** + * @name LoadCircleFootprint + * @brief Load a vector preview of the model's circle footprint + * @param footprint The footprint yaml node + * @param pose x,y,theta pose of footprint + */ + void LoadCircleFootprint(flatland_server::YamlReader &footprint, + const flatland_server::Pose pose); + + Ogre::Vector3 + intersection; // location cursor intersects ground plane, ie the + // location to create the model + float initial_angle; // the angle to create the model at + Ogre::SceneNode *moving_model_node_; // the node for the 3D object + enum ModelState { m_hidden, m_dragging, m_rotating }; + ModelState model_state; // model state, first hidden, then dragging to + // intersection point, then rotating to desired angle + static QString path_to_model_file_; // full path to model file (yaml) + static QString model_name; // base file name with path and extension removed + + protected: + rviz::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation + ros::NodeHandle nh; // ros service node handle + ros::ServiceClient client; // ros service client + std::vector> lines_list_; +}; + +} // end namespace flatland_viz + +#endif // SPAWN_MODEL_TOOL_H diff --git a/flatland_viz/package.xml b/flatland_viz/package.xml index 46d7a52b..4765ee7b 100644 --- a/flatland_viz/package.xml +++ b/flatland_viz/package.xml @@ -1,7 +1,7 @@ flatland_viz - 1.1.3 + 1.4.1 The flatland gui and visualization BSD https://bitbucket.org/avidbots/flatland diff --git a/flatland_viz/src/flatland_viz.cpp b/flatland_viz/src/flatland_viz.cpp index 74f87521..eef57f00 100644 --- a/flatland_viz/src/flatland_viz.cpp +++ b/flatland_viz/src/flatland_viz.cpp @@ -1,403 +1,403 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_viz.cpp - * @brief Manages the librviz viewport for flatland - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "rviz/display.h" -#include "rviz/render_panel.h" -#include "rviz/view_manager.h" -#include "rviz/visualization_manager.h" - -#include "flatland_viz/flatland_window.h" - -#include "flatland_viz/flatland_viz.h" - -// Constructor. -FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { - parent_ = parent; - toolbar_ = parent->addToolBar("Tools"); - - // init toolbar action handler - initToolbars(); - - initMenus(); - - // Construct and lay out render panel. - render_panel_ = new rviz::RenderPanel(); - QVBoxLayout* main_layout = new QVBoxLayout; - main_layout->setMargin(0); - main_layout->addWidget(render_panel_); - - // Set the top-level layout for this FlatlandViz widget. - setLayout(main_layout); - - // Next we initialize the main RViz classes. - // - // The VisualizationManager is the container for Display objects, - // holds the main Ogre scene, holds the ViewController, etc. It is - // very central and we will probably need one in every usage of - // librviz. - manager_ = new rviz::VisualizationManager(render_panel_); - render_panel_->initialize(manager_->getSceneManager(), manager_); - - // bind toolbar events - rviz::ToolManager* tool_man = manager_->getToolManager(); - - connect(manager_, SIGNAL(configChanged()), this, - SLOT(setDisplayConfigModified())); - connect(tool_man, &rviz::ToolManager::toolAdded, this, &FlatlandViz::addTool); - connect(tool_man, SIGNAL(toolRemoved(rviz::Tool*)), this, - SLOT(removeTool(rviz::Tool*))); - connect(tool_man, SIGNAL(toolRefreshed(rviz::Tool*)), this, - SLOT(refreshTool(rviz::Tool*))); - connect(tool_man, SIGNAL(toolChanged(rviz::Tool*)), this, - SLOT(indicateToolIsCurrent(rviz::Tool*))); - - manager_->initialize(); - - tool_man->addTool("flatland_viz/SpawnModel"); - tool_man->addTool("flatland_viz/PauseSim"); - - manager_->startUpdate(); - - // Set view controller to top down - manager_->getViewManager()->setCurrentViewControllerType("rviz/TopDownOrtho"); - render_panel_->setBackgroundColor(Ogre::ColourValue(0.2, 0.2, 0.2)); - - // Create a Grid display. - grid_ = manager_->createDisplay("rviz/Grid", "adjustable grid", true); - if (grid_ == nullptr) { - ROS_FATAL("Grid failed to instantiate"); - exit(1); - } - - // Configure the GridDisplay the way we like it. - grid_->subProp("Line Style")->setValue("Lines"); - grid_->subProp("Color")->setValue(QColor(Qt::white)); - grid_->subProp("Cell Size")->setValue(1.0); - grid_->subProp("Plane Cell Count")->setValue(100); - grid_->subProp("Alpha")->setValue(0.1); - - // Create interactive markers display - interactive_markers_ = - manager_->createDisplay("rviz/InteractiveMarkers", "Move Objects", false); - if (interactive_markers_ == nullptr) { - ROS_FATAL("Interactive markers failed to instantiate"); - exit(1); - } - interactive_markers_->subProp("Update Topic") - ->setValue("/interactive_model_markers/update"); - - // Subscribe to debug topics topic - ros::NodeHandle n; - debug_topic_subscriber_ = n.subscribe("/flatland_server/debug/topics", 0, - &FlatlandViz::RecieveDebugTopics, this); -} - -// Destructor. -FlatlandViz::~FlatlandViz() { - delete render_panel_; - delete manager_; -} - -void FlatlandViz::indicateToolIsCurrent(rviz::Tool* tool) { - QAction* action = tool_to_action_map_[tool]; - if (action) { - action->setChecked(true); - } -} - -void FlatlandViz::setDisplayConfigModified() { - ROS_ERROR("setDisplayConfigModified called"); -} - -void FlatlandViz::addTool(rviz::Tool* tool) { - ROS_ERROR("addTool called"); - QAction* action = new QAction(tool->getName(), toolbar_actions_); - action->setIcon(tool->getIcon()); - action->setIconText(tool->getName()); - action->setCheckable(true); - toolbar_->addAction(action); - action_to_tool_map_[action] = tool; - tool_to_action_map_[tool] = action; - - remove_tool_menu_->addAction(tool->getName()); -} - -void FlatlandViz::onToolbarActionTriggered(QAction* action) { - ROS_ERROR("onToolbarActionTriggered called"); - - rviz::Tool* current_tool = manager_->getToolManager()->getCurrentTool(); - rviz::Tool* tool = action_to_tool_map_[action]; - - if (tool) { - manager_->getToolManager()->setCurrentTool(tool); - - // If the simulation pause/resume tool was clicked, automatically and - // immediately switch back to the previously active tool - if (tool->getClassId().toStdString() == "flatland_viz/PauseSim") { - manager_->getToolManager()->setCurrentTool(current_tool); - tool = current_tool; - indicateToolIsCurrent(tool); - } - - // Show or hide interactive markers depending on whether interact mode is - // active - if (tool->getClassId().toStdString() == "rviz/Interact") { - interactive_markers_->setEnabled(true); - } else { - interactive_markers_->setEnabled(false); - } - } -} - -void FlatlandViz::removeTool(rviz::Tool* tool) { - ROS_ERROR("removeTool called"); - QAction* action = tool_to_action_map_[tool]; - if (action) { - toolbar_actions_->removeAction(action); - toolbar_->removeAction(action); - tool_to_action_map_.erase(tool); - action_to_tool_map_.erase(action); - } - QString tool_name = tool->getName(); - QList remove_tool_actions = remove_tool_menu_->actions(); - for (int i = 0; i < remove_tool_actions.size(); i++) { - ROS_ERROR_STREAM("Removing --------> " << tool_name.toStdString()); - QAction* removal_action = remove_tool_actions.at(i); - if (removal_action->text() == tool_name) { - remove_tool_menu_->removeAction(removal_action); - break; - } - } -} - -void FlatlandViz::initMenus() { - file_menu_ = parent_->menuBar()->addMenu("&File"); - - QAction* file_menu_open_action = file_menu_->addAction( - "&Open Config", this, SLOT(onOpen()), QKeySequence("Ctrl+O")); - this->addAction(file_menu_open_action); - QAction* file_menu_save_action = file_menu_->addAction( - "&Save Config", this, SLOT(onSave()), QKeySequence("Ctrl+S")); - this->addAction(file_menu_save_action); - QAction* file_menu_save_as_action = file_menu_->addAction( - "Save Config &As", this, SLOT(onSaveAs()), QKeySequence("Ctrl+Shift+S")); - this->addAction(file_menu_save_as_action); - - recent_configs_menu_ = file_menu_->addMenu("&Recent Configs"); - file_menu_->addAction("Save &Image", this, SLOT(onSaveImage())); - if (show_choose_new_master_option_) { - file_menu_->addSeparator(); - file_menu_->addAction("Change &Master", this, SLOT(changeMaster())); - } - file_menu_->addSeparator(); - - QAction* file_menu_quit_action = file_menu_->addAction( - "&Quit", this, SLOT(close()), QKeySequence("Ctrl+Q")); - this->addAction(file_menu_quit_action); - - view_menu_ = parent_->menuBar()->addMenu("&Panels"); - view_menu_->addAction("Add &New Panel", this, SLOT(openNewPanelDialog())); - delete_view_menu_ = view_menu_->addMenu("&Delete Panel"); - delete_view_menu_->setEnabled(false); - - QAction* fullscreen_action = view_menu_->addAction( - "&Fullscreen", this, SLOT(setFullScreen(bool)), Qt::Key_F11); - fullscreen_action->setCheckable(true); - this->addAction(fullscreen_action); // Also add to window, or the shortcut - // doest work when the menu is hidden. - - // connect(this, SIGNAL(fullScreenChange(bool)), fullscreen_action, - // SLOT(setChecked(bool))); - new QShortcut(Qt::Key_Escape, this, SLOT(exitFullScreen())); - view_menu_->addSeparator(); - - QMenu* help_menu = parent_->menuBar()->addMenu("&Help"); - help_menu->addAction("Show &Help panel", this, SLOT(showHelpPanel())); - help_menu->addAction("Open rviz wiki in browser", this, SLOT(onHelpWiki())); - help_menu->addSeparator(); - help_menu->addAction("&About", this, SLOT(onHelpAbout())); -} - -void FlatlandViz::initToolbars() { - QFont font; - font.setPointSize(font.pointSizeF() * 0.9); - - // make toolbar with plugin tools - - toolbar_->setFont(font); - toolbar_->setContentsMargins(0, 0, 0, 0); - toolbar_->setObjectName("Tools"); - toolbar_->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); - toolbar_actions_ = new QActionGroup(this); - connect(toolbar_actions_, &QActionGroup::triggered, this, - &FlatlandViz::onToolbarActionTriggered); - - add_tool_action_ = new QAction("", toolbar_actions_); - add_tool_action_->setToolTip("Add a new tool"); - add_tool_action_->setIcon(rviz::loadPixmap("package://rviz/icons/plus.png")); - toolbar_->addAction(add_tool_action_); - - connect(add_tool_action_, &QAction::triggered, this, - &FlatlandViz::openNewToolDialog); - - remove_tool_menu_ = new QMenu(); - QToolButton* remove_tool_button = new QToolButton(); - remove_tool_button->setMenu(remove_tool_menu_); - remove_tool_button->setPopupMode(QToolButton::InstantPopup); - remove_tool_button->setToolTip("Remove a tool from the toolbar"); - remove_tool_button->setIcon( - rviz::loadPixmap("package://rviz/icons/minus.png")); - toolbar_->addWidget(remove_tool_button); - - connect(remove_tool_menu_, &QMenu::triggered, this, - &FlatlandViz::onToolbarRemoveTool); -} - -void FlatlandViz::openNewToolDialog() { - ROS_ERROR("openNewToolDialog called"); - QString class_id; - QStringList empty; - rviz::ToolManager* tool_man = manager_->getToolManager(); - - rviz::NewObjectDialog* dialog = - new rviz::NewObjectDialog(tool_man->getFactory(), "Tool", empty, - tool_man->getToolClasses(), &class_id); - manager_->stopUpdate(); - if (dialog->exec() == QDialog::Accepted) { - tool_man->addTool(class_id); - } - manager_->startUpdate(); - activateWindow(); // Force keyboard focus back on main window. -} - -void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { - ROS_ERROR("onToolbarRemoveTool called"); - QString name = remove_tool_menu_action->text(); - for (int i = 0; i < manager_->getToolManager()->numTools(); i++) { - rviz::Tool* tool = manager_->getToolManager()->getTool(i); - if (tool->getName() == name) { - ROS_ERROR_STREAM("Removing --------> " << name.toStdString()); - manager_->getToolManager()->removeTool(i); - removeTool(tool); - return; - } - } -} - -void FlatlandViz::refreshTool(rviz::Tool* tool) { - QAction* action = tool_to_action_map_[tool]; - action->setIcon(tool->getIcon()); - action->setIconText(tool->getName()); -} - -void FlatlandViz::setFullScreen(bool full_screen) { - // Q_EMIT(fullScreenChange(full_screen)); - - if (full_screen) toolbar_visible_ = toolbar_->isVisible(); - parent_->menuBar()->setVisible(!full_screen); - toolbar_->setVisible(!full_screen && toolbar_visible_); - parent_->statusBar()->setVisible(!full_screen); - - if (full_screen) - setWindowState(windowState() | Qt::WindowFullScreen); - else - setWindowState(windowState() & ~Qt::WindowFullScreen); - show(); -} - -void FlatlandViz::RecieveDebugTopics(const flatland_msgs::DebugTopicList& msg) { - std::vector topics = msg.topics; - - // check for deleted topics - for (auto& topic : debug_displays_) { - if (std::count(topics.begin(), topics.end(), topic.first) == 0) { - delete debug_displays_[topic.first]; - debug_displays_.erase(topic.first); - } - } - - // check for new topics - for (const auto& topic : topics) { - if (debug_displays_.count(topic) == 0) { - // Create the marker display and set its topic - debug_displays_[topic] = manager_->createDisplay( - "rviz/MarkerArray", QString::fromLocal8Bit(topic.c_str()), true); - if (debug_displays_[topic] == nullptr) { - ROS_FATAL("MarkerArray failed to instantiate"); - exit(1); - } - QString topic_qt = QString::fromLocal8Bit( - (std::string("/flatland_server/debug/") + topic).c_str()); - debug_displays_[topic]->subProp("Marker Topic")->setValue(topic_qt); - } - } -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_viz.cpp + * @brief Manages the librviz viewport for flatland + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "rviz/display.h" +#include "rviz/render_panel.h" +#include "rviz/view_manager.h" +#include "rviz/visualization_manager.h" + +#include "flatland_viz/flatland_window.h" + +#include "flatland_viz/flatland_viz.h" + +// Constructor. +FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { + parent_ = parent; + toolbar_ = parent->addToolBar("Tools"); + + // init toolbar action handler + initToolbars(); + + initMenus(); + + // Construct and lay out render panel. + render_panel_ = new rviz::RenderPanel(); + QVBoxLayout* main_layout = new QVBoxLayout; + main_layout->setMargin(0); + main_layout->addWidget(render_panel_); + + // Set the top-level layout for this FlatlandViz widget. + setLayout(main_layout); + + // Next we initialize the main RViz classes. + // + // The VisualizationManager is the container for Display objects, + // holds the main Ogre scene, holds the ViewController, etc. It is + // very central and we will probably need one in every usage of + // librviz. + manager_ = new rviz::VisualizationManager(render_panel_); + render_panel_->initialize(manager_->getSceneManager(), manager_); + + // bind toolbar events + rviz::ToolManager* tool_man = manager_->getToolManager(); + + connect(manager_, SIGNAL(configChanged()), this, + SLOT(setDisplayConfigModified())); + connect(tool_man, &rviz::ToolManager::toolAdded, this, &FlatlandViz::addTool); + connect(tool_man, SIGNAL(toolRemoved(rviz::Tool*)), this, + SLOT(removeTool(rviz::Tool*))); + connect(tool_man, SIGNAL(toolRefreshed(rviz::Tool*)), this, + SLOT(refreshTool(rviz::Tool*))); + connect(tool_man, SIGNAL(toolChanged(rviz::Tool*)), this, + SLOT(indicateToolIsCurrent(rviz::Tool*))); + + manager_->initialize(); + + tool_man->addTool("flatland_viz/SpawnModel"); + tool_man->addTool("flatland_viz/PauseSim"); + + manager_->startUpdate(); + + // Set view controller to top down + manager_->getViewManager()->setCurrentViewControllerType("rviz/TopDownOrtho"); + render_panel_->setBackgroundColor(Ogre::ColourValue(0.2, 0.2, 0.2)); + + // Create a Grid display. + grid_ = manager_->createDisplay("rviz/Grid", "adjustable grid", true); + if (grid_ == nullptr) { + ROS_FATAL("Grid failed to instantiate"); + exit(1); + } + + // Configure the GridDisplay the way we like it. + grid_->subProp("Line Style")->setValue("Lines"); + grid_->subProp("Color")->setValue(QColor(Qt::white)); + grid_->subProp("Cell Size")->setValue(1.0); + grid_->subProp("Plane Cell Count")->setValue(100); + grid_->subProp("Alpha")->setValue(0.1); + + // Create interactive markers display + interactive_markers_ = + manager_->createDisplay("rviz/InteractiveMarkers", "Move Objects", false); + if (interactive_markers_ == nullptr) { + ROS_FATAL("Interactive markers failed to instantiate"); + exit(1); + } + interactive_markers_->subProp("Update Topic") + ->setValue("/interactive_model_markers/update"); + + // Subscribe to debug topics topic + ros::NodeHandle n; + debug_topic_subscriber_ = n.subscribe("/flatland_server/debug/topics", 0, + &FlatlandViz::RecieveDebugTopics, this); +} + +// Destructor. +FlatlandViz::~FlatlandViz() { + delete render_panel_; + delete manager_; +} + +void FlatlandViz::indicateToolIsCurrent(rviz::Tool* tool) { + QAction* action = tool_to_action_map_[tool]; + if (action) { + action->setChecked(true); + } +} + +void FlatlandViz::setDisplayConfigModified() { + ROS_ERROR("setDisplayConfigModified called"); +} + +void FlatlandViz::addTool(rviz::Tool* tool) { + ROS_ERROR("addTool called"); + QAction* action = new QAction(tool->getName(), toolbar_actions_); + action->setIcon(tool->getIcon()); + action->setIconText(tool->getName()); + action->setCheckable(true); + toolbar_->addAction(action); + action_to_tool_map_[action] = tool; + tool_to_action_map_[tool] = action; + + remove_tool_menu_->addAction(tool->getName()); +} + +void FlatlandViz::onToolbarActionTriggered(QAction* action) { + ROS_ERROR("onToolbarActionTriggered called"); + + rviz::Tool* current_tool = manager_->getToolManager()->getCurrentTool(); + rviz::Tool* tool = action_to_tool_map_[action]; + + if (tool) { + manager_->getToolManager()->setCurrentTool(tool); + + // If the simulation pause/resume tool was clicked, automatically and + // immediately switch back to the previously active tool + if (tool->getClassId().toStdString() == "flatland_viz/PauseSim") { + manager_->getToolManager()->setCurrentTool(current_tool); + tool = current_tool; + indicateToolIsCurrent(tool); + } + + // Show or hide interactive markers depending on whether interact mode is + // active + if (tool->getClassId().toStdString() == "rviz/Interact") { + interactive_markers_->setEnabled(true); + } else { + interactive_markers_->setEnabled(false); + } + } +} + +void FlatlandViz::removeTool(rviz::Tool* tool) { + ROS_ERROR("removeTool called"); + QAction* action = tool_to_action_map_[tool]; + if (action) { + toolbar_actions_->removeAction(action); + toolbar_->removeAction(action); + tool_to_action_map_.erase(tool); + action_to_tool_map_.erase(action); + } + QString tool_name = tool->getName(); + QList remove_tool_actions = remove_tool_menu_->actions(); + for (int i = 0; i < remove_tool_actions.size(); i++) { + ROS_ERROR_STREAM("Removing --------> " << tool_name.toStdString()); + QAction* removal_action = remove_tool_actions.at(i); + if (removal_action->text() == tool_name) { + remove_tool_menu_->removeAction(removal_action); + break; + } + } +} + +void FlatlandViz::initMenus() { + file_menu_ = parent_->menuBar()->addMenu("&File"); + + QAction* file_menu_open_action = file_menu_->addAction( + "&Open Config", this, SLOT(onOpen()), QKeySequence("Ctrl+O")); + this->addAction(file_menu_open_action); + QAction* file_menu_save_action = file_menu_->addAction( + "&Save Config", this, SLOT(onSave()), QKeySequence("Ctrl+S")); + this->addAction(file_menu_save_action); + QAction* file_menu_save_as_action = file_menu_->addAction( + "Save Config &As", this, SLOT(onSaveAs()), QKeySequence("Ctrl+Shift+S")); + this->addAction(file_menu_save_as_action); + + recent_configs_menu_ = file_menu_->addMenu("&Recent Configs"); + file_menu_->addAction("Save &Image", this, SLOT(onSaveImage())); + if (show_choose_new_master_option_) { + file_menu_->addSeparator(); + file_menu_->addAction("Change &Master", this, SLOT(changeMaster())); + } + file_menu_->addSeparator(); + + QAction* file_menu_quit_action = file_menu_->addAction( + "&Quit", this, SLOT(close()), QKeySequence("Ctrl+Q")); + this->addAction(file_menu_quit_action); + + view_menu_ = parent_->menuBar()->addMenu("&Panels"); + view_menu_->addAction("Add &New Panel", this, SLOT(openNewPanelDialog())); + delete_view_menu_ = view_menu_->addMenu("&Delete Panel"); + delete_view_menu_->setEnabled(false); + + QAction* fullscreen_action = view_menu_->addAction( + "&Fullscreen", this, SLOT(setFullScreen(bool)), Qt::Key_F11); + fullscreen_action->setCheckable(true); + this->addAction(fullscreen_action); // Also add to window, or the shortcut + // doest work when the menu is hidden. + + // connect(this, SIGNAL(fullScreenChange(bool)), fullscreen_action, + // SLOT(setChecked(bool))); + new QShortcut(Qt::Key_Escape, this, SLOT(exitFullScreen())); + view_menu_->addSeparator(); + + QMenu* help_menu = parent_->menuBar()->addMenu("&Help"); + help_menu->addAction("Show &Help panel", this, SLOT(showHelpPanel())); + help_menu->addAction("Open rviz wiki in browser", this, SLOT(onHelpWiki())); + help_menu->addSeparator(); + help_menu->addAction("&About", this, SLOT(onHelpAbout())); +} + +void FlatlandViz::initToolbars() { + QFont font; + font.setPointSize(font.pointSizeF() * 0.9); + + // make toolbar with plugin tools + + toolbar_->setFont(font); + toolbar_->setContentsMargins(0, 0, 0, 0); + toolbar_->setObjectName("Tools"); + toolbar_->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); + toolbar_actions_ = new QActionGroup(this); + connect(toolbar_actions_, &QActionGroup::triggered, this, + &FlatlandViz::onToolbarActionTriggered); + + add_tool_action_ = new QAction("", toolbar_actions_); + add_tool_action_->setToolTip("Add a new tool"); + add_tool_action_->setIcon(rviz::loadPixmap("package://rviz/icons/plus.png")); + toolbar_->addAction(add_tool_action_); + + connect(add_tool_action_, &QAction::triggered, this, + &FlatlandViz::openNewToolDialog); + + remove_tool_menu_ = new QMenu(); + QToolButton* remove_tool_button = new QToolButton(); + remove_tool_button->setMenu(remove_tool_menu_); + remove_tool_button->setPopupMode(QToolButton::InstantPopup); + remove_tool_button->setToolTip("Remove a tool from the toolbar"); + remove_tool_button->setIcon( + rviz::loadPixmap("package://rviz/icons/minus.png")); + toolbar_->addWidget(remove_tool_button); + + connect(remove_tool_menu_, &QMenu::triggered, this, + &FlatlandViz::onToolbarRemoveTool); +} + +void FlatlandViz::openNewToolDialog() { + ROS_ERROR("openNewToolDialog called"); + QString class_id; + QStringList empty; + rviz::ToolManager* tool_man = manager_->getToolManager(); + + rviz::NewObjectDialog* dialog = + new rviz::NewObjectDialog(tool_man->getFactory(), "Tool", empty, + tool_man->getToolClasses(), &class_id); + manager_->stopUpdate(); + if (dialog->exec() == QDialog::Accepted) { + tool_man->addTool(class_id); + } + manager_->startUpdate(); + activateWindow(); // Force keyboard focus back on main window. +} + +void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { + ROS_ERROR("onToolbarRemoveTool called"); + QString name = remove_tool_menu_action->text(); + for (int i = 0; i < manager_->getToolManager()->numTools(); i++) { + rviz::Tool* tool = manager_->getToolManager()->getTool(i); + if (tool->getName() == name) { + ROS_ERROR_STREAM("Removing --------> " << name.toStdString()); + manager_->getToolManager()->removeTool(i); + removeTool(tool); + return; + } + } +} + +void FlatlandViz::refreshTool(rviz::Tool* tool) { + QAction* action = tool_to_action_map_[tool]; + action->setIcon(tool->getIcon()); + action->setIconText(tool->getName()); +} + +void FlatlandViz::setFullScreen(bool full_screen) { + // Q_EMIT(fullScreenChange(full_screen)); + + if (full_screen) toolbar_visible_ = toolbar_->isVisible(); + parent_->menuBar()->setVisible(!full_screen); + toolbar_->setVisible(!full_screen && toolbar_visible_); + parent_->statusBar()->setVisible(!full_screen); + + if (full_screen) + setWindowState(windowState() | Qt::WindowFullScreen); + else + setWindowState(windowState() & ~Qt::WindowFullScreen); + show(); +} + +void FlatlandViz::RecieveDebugTopics(const flatland_msgs::DebugTopicList& msg) { + std::vector topics = msg.topics; + + // check for deleted topics + for (auto& topic : debug_displays_) { + if (std::count(topics.begin(), topics.end(), topic.first) == 0) { + delete debug_displays_[topic.first]; + debug_displays_.erase(topic.first); + } + } + + // check for new topics + for (const auto& topic : topics) { + if (debug_displays_.count(topic) == 0) { + // Create the marker display and set its topic + debug_displays_[topic] = manager_->createDisplay( + "rviz/MarkerArray", QString::fromLocal8Bit(topic.c_str()), true); + if (debug_displays_[topic] == nullptr) { + ROS_FATAL("MarkerArray failed to instantiate"); + exit(1); + } + QString topic_qt = QString::fromLocal8Bit( + (std::string("/flatland_server/debug/") + topic).c_str()); + debug_displays_[topic]->subProp("Marker Topic")->setValue(topic_qt); + } + } +} diff --git a/flatland_viz/src/flatland_viz_node.cpp b/flatland_viz/src/flatland_viz_node.cpp index d872d894..cb1adfdf 100644 --- a/flatland_viz/src/flatland_viz_node.cpp +++ b/flatland_viz/src/flatland_viz_node.cpp @@ -1,88 +1,88 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_viz_node.cpp - * @brief The main ROS node for flatland_viz - * @author Joseph Duchesne - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include "flatland_viz/flatland_window.h" - -FlatlandWindow* window = nullptr; - -/** - * @name SigintHandler - * @brief Interrupt handler - sends shutdown signal to simulation_manager - * @param[in] sig: signal itself - */ -void SigintHandler(int sig) { - ROS_WARN_NAMED("Node", "*** Shutting down... ***"); - - if (window != nullptr) { - delete window; - window = nullptr; - } - ROS_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); - ros::shutdown(); -} - -int main(int argc, char** argv) { - if (!ros::isInitialized()) { - ros::init(argc, argv, "flatland_viz", ros::init_options::NoSigintHandler); - } - - QApplication app(argc, argv); - - window = new FlatlandWindow(); - window->show(); - - // Register sigint shutdown handler - signal(SIGINT, SigintHandler); - - app.exec(); - - delete window; - window = nullptr; - return 0; -} \ No newline at end of file +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_viz_node.cpp + * @brief The main ROS node for flatland_viz + * @author Joseph Duchesne + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include "flatland_viz/flatland_window.h" + +FlatlandWindow* window = nullptr; + +/** + * @name SigintHandler + * @brief Interrupt handler - sends shutdown signal to simulation_manager + * @param[in] sig: signal itself + */ +void SigintHandler(int sig) { + ROS_WARN_NAMED("Node", "*** Shutting down... ***"); + + if (window != nullptr) { + delete window; + window = nullptr; + } + ROS_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); + ros::shutdown(); +} + +int main(int argc, char** argv) { + if (!ros::isInitialized()) { + ros::init(argc, argv, "flatland_viz", ros::init_options::NoSigintHandler); + } + + QApplication app(argc, argv); + + window = new FlatlandWindow(); + window->show(); + + // Register sigint shutdown handler + signal(SIGINT, SigintHandler); + + app.exec(); + + delete window; + window = nullptr; + return 0; +} diff --git a/flatland_viz/src/flatland_window.cpp b/flatland_viz/src/flatland_window.cpp index d2e8d78b..d8d5acb2 100644 --- a/flatland_viz/src/flatland_window.cpp +++ b/flatland_viz/src/flatland_window.cpp @@ -1,113 +1,113 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name flatland_window.cpp - * @brief Main window and toolbars for flatland_viz - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "flatland_viz/flatland_window.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rviz/display.h" -#include "rviz/display_context.h" -#include "rviz/displays_panel.h" -#include "rviz/env_config.h" -#include "rviz/failed_panel.h" -#include "rviz/help_panel.h" -#include "rviz/load_resource.h" -#include "rviz/loading_dialog.h" -#include "rviz/new_object_dialog.h" -#include "rviz/panel_dock_widget.h" -#include "rviz/panel_factory.h" -#include "rviz/properties/status_property.h" -#include "rviz/render_panel.h" -#include "rviz/screenshot_dialog.h" -#include "rviz/selection/selection_manager.h" -#include "rviz/selection_panel.h" -#include "rviz/splash_screen.h" -#include "rviz/time_panel.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/tool_properties_panel.h" -#include "rviz/view_manager.h" -#include "rviz/views_panel.h" -#include "rviz/visualization_frame.h" -#include "rviz/visualization_manager.h" -#include "rviz/widget_geometry_change_detector.h" -#include "rviz/yaml_config_reader.h" -#include "rviz/yaml_config_writer.h" - -#include -#include -//#include - -void FlatlandWindow::openNewToolDialog() { - QString class_id; - QStringList empty; -} - -rviz::VisualizationManager *FlatlandWindow::getManager() { - return visualization_manager_; -} - -FlatlandWindow::FlatlandWindow(QWidget *parent) : QMainWindow(parent) { - // Create the main viewport - viz_ = new FlatlandViz(this); - setCentralWidget(viz_); - resize(QDesktopWidget().availableGeometry(this).size() * 0.9); - - // Set the main window properties - setWindowTitle("Flatland Viz"); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name flatland_window.cpp + * @brief Main window and toolbars for flatland_viz + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "flatland_viz/flatland_window.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rviz/display.h" +#include "rviz/display_context.h" +#include "rviz/displays_panel.h" +#include "rviz/env_config.h" +#include "rviz/failed_panel.h" +#include "rviz/help_panel.h" +#include "rviz/load_resource.h" +#include "rviz/loading_dialog.h" +#include "rviz/new_object_dialog.h" +#include "rviz/panel_dock_widget.h" +#include "rviz/panel_factory.h" +#include "rviz/properties/status_property.h" +#include "rviz/render_panel.h" +#include "rviz/screenshot_dialog.h" +#include "rviz/selection/selection_manager.h" +#include "rviz/selection_panel.h" +#include "rviz/splash_screen.h" +#include "rviz/time_panel.h" +#include "rviz/tool.h" +#include "rviz/tool_manager.h" +#include "rviz/tool_properties_panel.h" +#include "rviz/view_manager.h" +#include "rviz/views_panel.h" +#include "rviz/visualization_frame.h" +#include "rviz/visualization_manager.h" +#include "rviz/widget_geometry_change_detector.h" +#include "rviz/yaml_config_reader.h" +#include "rviz/yaml_config_writer.h" + +#include +#include +//#include + +void FlatlandWindow::openNewToolDialog() { + QString class_id; + QStringList empty; +} + +rviz::VisualizationManager *FlatlandWindow::getManager() { + return visualization_manager_; +} + +FlatlandWindow::FlatlandWindow(QWidget *parent) : QMainWindow(parent) { + // Create the main viewport + viz_ = new FlatlandViz(this); + setCentralWidget(viz_); + resize(QDesktopWidget().availableGeometry(this).size() * 0.9); + + // Set the main window properties + setWindowTitle("Flatland Viz"); +} diff --git a/flatland_viz/src/load_model_dialog.cpp b/flatland_viz/src/load_model_dialog.cpp index e2a70af8..bb29508a 100644 --- a/flatland_viz/src/load_model_dialog.cpp +++ b/flatland_viz/src/load_model_dialog.cpp @@ -1,221 +1,221 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name load_model_dialog.cpp - * @brief Spawn dialog - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "flatland_viz/load_model_dialog.h" -#include "flatland_viz/spawn_model_tool.h" -// #include "load_model_dialog.h" -// #include "spawn_model_tool.h" - -QString LoadModelDialog::path_to_model_file; -int LoadModelDialog::count; -bool LoadModelDialog::numbering; - -LoadModelDialog::LoadModelDialog(QWidget *parent, - flatland_viz::SpawnModelTool *tool) - : QDialog(parent), tool_(tool) { - ROS_INFO_STREAM("ModelDialog::ModelDialog"); - QVBoxLayout *v_layout = new QVBoxLayout; - setLayout(v_layout); - - // we are injecting horizontal layouts into the master vertical layout - QHBoxLayout *h0_layout = new QHBoxLayout; - QHBoxLayout *h1_layout = new QHBoxLayout; - QHBoxLayout *h2_layout = new QHBoxLayout; - QHBoxLayout *h3_layout = new QHBoxLayout; - - // create widgets - QPushButton *pathButton = new QPushButton("choose file"); - p_label = new QLabel; - n_checkbox = new QCheckBox; - n_edit = new QLineEdit; - QPushButton *okButton = new QPushButton("ok"); - QPushButton *cancelButton = new QPushButton("cancel"); - - // set focus policy, otherwise cr in textfield triggers all the slots - pathButton->setFocusPolicy(Qt::NoFocus); - p_label->setFocusPolicy(Qt::NoFocus); - n_checkbox->setFocusPolicy(Qt::NoFocus); - n_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus - okButton->setFocusPolicy(Qt::NoFocus); - cancelButton->setFocusPolicy(Qt::NoFocus); - - connect(pathButton, &QAbstractButton::clicked, this, - &LoadModelDialog::on_PathButtonClicked); - connect(okButton, &QAbstractButton::clicked, this, - &LoadModelDialog::OkButtonClicked); - connect(cancelButton, &QAbstractButton::clicked, this, - &LoadModelDialog::CancelButtonClicked); - connect(n_checkbox, &QAbstractButton::clicked, this, - &LoadModelDialog::NumberCheckBoxChanged); - - // path button - h0_layout->addWidget(pathButton); - - // path label - p_label->setText(path_to_model_file); - h1_layout->addWidget(new QLabel("path:")); - h1_layout->addWidget(p_label); - - // - h2_layout->addWidget(new QLabel("number:")); - h2_layout->addWidget(n_checkbox); - n_checkbox->setChecked(numbering); - h2_layout->addWidget(new QLabel("name:")); - h2_layout->addWidget(n_edit); - - // set the default name to the filename parsed using boost - AddNumberAndUpdateName(); - - // ok button - h3_layout->addWidget(okButton); - - // cancel button - h3_layout->addWidget(cancelButton); - - // add the horizontal layouts to the vertical layout - v_layout->addLayout(h0_layout); - v_layout->addLayout(h1_layout); - v_layout->addLayout(h2_layout); - v_layout->addLayout(h3_layout); - - // set the top level layout - setLayout(v_layout); - - // delete the Dialog if the user clicks on the x to close window - this->setAttribute(Qt::WA_DeleteOnClose, true); -} - -void LoadModelDialog::CancelButtonClicked() { - ROS_INFO_STREAM("LoadModelDialog::CancelButtonClicked"); - this->close(); -} - -void LoadModelDialog::OkButtonClicked() { - ROS_INFO_STREAM("LoadModelDialog::OkButtonClicked"); - - QString name = n_edit->displayText(); - - tool_->SaveName(name); - tool_->SavePath(path_to_model_file); - tool_->BeginPlacement(); - - this->close(); -} - -void LoadModelDialog::AddNumberAndUpdateName() { - std::string bsfn = - boost::filesystem::basename(path_to_model_file.toStdString()); - QString name = QString::fromStdString(bsfn); - - if (numbering) { - name = name.append(QString::number(count++)); - } - - // update the name text field - n_edit->setText(name); -} - -void LoadModelDialog::on_PathButtonClicked() { - ROS_INFO_STREAM("LoadModelDialog::on_PathButtonClicked"); - path_to_model_file = ChooseFile(); - - AddNumberAndUpdateName(); - p_label->setText(path_to_model_file); - n_edit->setFocus(); -} - -void LoadModelDialog::NumberCheckBoxChanged(bool i) { - ROS_INFO_STREAM("NumberCheckBoxChanged"); - numbering = !numbering; - AddNumberAndUpdateName(); -} - -QString LoadModelDialog::ChooseFile() { - QString fileName = - QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); - if (fileName.isEmpty()) - return fileName; - else { - QFile file(fileName); - - if (!file.open(QIODevice::ReadOnly)) { - QMessageBox::information(NULL, tr("Unable to open file"), - file.errorString()); - return fileName; - } - file.close(); - - return fileName; - } -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name load_model_dialog.cpp + * @brief Spawn dialog + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "flatland_viz/load_model_dialog.h" +#include "flatland_viz/spawn_model_tool.h" +// #include "load_model_dialog.h" +// #include "spawn_model_tool.h" + +QString LoadModelDialog::path_to_model_file; +int LoadModelDialog::count; +bool LoadModelDialog::numbering; + +LoadModelDialog::LoadModelDialog(QWidget *parent, + flatland_viz::SpawnModelTool *tool) + : QDialog(parent), tool_(tool) { + ROS_INFO_STREAM("ModelDialog::ModelDialog"); + QVBoxLayout *v_layout = new QVBoxLayout; + setLayout(v_layout); + + // we are injecting horizontal layouts into the master vertical layout + QHBoxLayout *h0_layout = new QHBoxLayout; + QHBoxLayout *h1_layout = new QHBoxLayout; + QHBoxLayout *h2_layout = new QHBoxLayout; + QHBoxLayout *h3_layout = new QHBoxLayout; + + // create widgets + QPushButton *pathButton = new QPushButton("choose file"); + p_label = new QLabel; + n_checkbox = new QCheckBox; + n_edit = new QLineEdit; + QPushButton *okButton = new QPushButton("ok"); + QPushButton *cancelButton = new QPushButton("cancel"); + + // set focus policy, otherwise cr in textfield triggers all the slots + pathButton->setFocusPolicy(Qt::NoFocus); + p_label->setFocusPolicy(Qt::NoFocus); + n_checkbox->setFocusPolicy(Qt::NoFocus); + n_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus + okButton->setFocusPolicy(Qt::NoFocus); + cancelButton->setFocusPolicy(Qt::NoFocus); + + connect(pathButton, &QAbstractButton::clicked, this, + &LoadModelDialog::on_PathButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, + &LoadModelDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, + &LoadModelDialog::CancelButtonClicked); + connect(n_checkbox, &QAbstractButton::clicked, this, + &LoadModelDialog::NumberCheckBoxChanged); + + // path button + h0_layout->addWidget(pathButton); + + // path label + p_label->setText(path_to_model_file); + h1_layout->addWidget(new QLabel("path:")); + h1_layout->addWidget(p_label); + + // + h2_layout->addWidget(new QLabel("number:")); + h2_layout->addWidget(n_checkbox); + n_checkbox->setChecked(numbering); + h2_layout->addWidget(new QLabel("name:")); + h2_layout->addWidget(n_edit); + + // set the default name to the filename parsed using boost + AddNumberAndUpdateName(); + + // ok button + h3_layout->addWidget(okButton); + + // cancel button + h3_layout->addWidget(cancelButton); + + // add the horizontal layouts to the vertical layout + v_layout->addLayout(h0_layout); + v_layout->addLayout(h1_layout); + v_layout->addLayout(h2_layout); + v_layout->addLayout(h3_layout); + + // set the top level layout + setLayout(v_layout); + + // delete the Dialog if the user clicks on the x to close window + this->setAttribute(Qt::WA_DeleteOnClose, true); +} + +void LoadModelDialog::CancelButtonClicked() { + ROS_INFO_STREAM("LoadModelDialog::CancelButtonClicked"); + this->close(); +} + +void LoadModelDialog::OkButtonClicked() { + ROS_INFO_STREAM("LoadModelDialog::OkButtonClicked"); + + QString name = n_edit->displayText(); + + tool_->SaveName(name); + tool_->SavePath(path_to_model_file); + tool_->BeginPlacement(); + + this->close(); +} + +void LoadModelDialog::AddNumberAndUpdateName() { + std::string bsfn = + boost::filesystem::basename(path_to_model_file.toStdString()); + QString name = QString::fromStdString(bsfn); + + if (numbering) { + name = name.append(QString::number(count++)); + } + + // update the name text field + n_edit->setText(name); +} + +void LoadModelDialog::on_PathButtonClicked() { + ROS_INFO_STREAM("LoadModelDialog::on_PathButtonClicked"); + path_to_model_file = ChooseFile(); + + AddNumberAndUpdateName(); + p_label->setText(path_to_model_file); + n_edit->setFocus(); +} + +void LoadModelDialog::NumberCheckBoxChanged(bool i) { + ROS_INFO_STREAM("NumberCheckBoxChanged"); + numbering = !numbering; + AddNumberAndUpdateName(); +} + +QString LoadModelDialog::ChooseFile() { + QString fileName = + QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); + if (fileName.isEmpty()) + return fileName; + else { + QFile file(fileName); + + if (!file.open(QIODevice::ReadOnly)) { + QMessageBox::information(NULL, tr("Unable to open file"), + file.errorString()); + return fileName; + } + file.close(); + + return fileName; + } +} diff --git a/flatland_viz/src/model_dialog.cpp b/flatland_viz/src/model_dialog.cpp index 2f200e64..a5fc65ab 100644 --- a/flatland_viz/src/model_dialog.cpp +++ b/flatland_viz/src/model_dialog.cpp @@ -1,224 +1,224 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2017 Avidbots Corp. - * @name model_dialog.cpp - * @brief Main window and toolbars for flatland_viz - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include -#include -#include - -#include "flatland_viz/model_dialog.h" - -// Initialize static variables -QColor ModelDialog::saved_color_; - -QString ModelDialog::SelectFile() { - QString fileName = - QFileDialog::getOpenFileName(this, tr("Open model file"), "", ""); - if (fileName.isEmpty()) - return fileName; - else { - QFile file(fileName); - - if (!file.open(QIODevice::ReadOnly)) { - QMessageBox::information(this, tr("Unable to open file"), - file.errorString()); - return fileName; - } - file.close(); - - return fileName; - } -} - -ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { - ROS_ERROR_STREAM("ModelDialog::ModelDialog"); - - path_to_model_file = SelectFile(); - QVBoxLayout *v_layout = new QVBoxLayout; - - // we are injecting horizontal layouts into the master vertical layout - QHBoxLayout *h1_layout = new QHBoxLayout; - QHBoxLayout *h2_layout = new QHBoxLayout; - QHBoxLayout *h3_layout = new QHBoxLayout; - QHBoxLayout *h4_layout = new QHBoxLayout; - QHBoxLayout *h5_layout = new QHBoxLayout; - - QLabel *colorLabel = new QLabel; - int frameStyle = QFrame::Sunken | QFrame::Panel; - colorLabel->setFrameStyle(frameStyle); - color_button = new QPushButton(""); - QPushButton *okButton = new QPushButton("ok"); - QPushButton *cancelButton = new QPushButton("cancel"); - connect(color_button, &QAbstractButton::clicked, this, - &ModelDialog::SetColor); - - // path label and path - QLabel *path = new QLabel; - path->setText(path_to_model_file); - h1_layout->addWidget(new QLabel("path:")); - h1_layout->addWidget(path); - - // name label and name LineEdit - n_edit = new QLineEdit; - h2_layout->addWidget(new QLabel("name:")); - h2_layout->addWidget(n_edit); - - // set the default name to the filename parsed using boost - std::string bsfn = - boost::filesystem::basename(path_to_model_file.toStdString()); - QString fn = QString::fromStdString(bsfn); - n_edit->setText(fn); - - // color label and button - h3_layout->addWidget(new QLabel("color")); - h3_layout->addWidget(color_button); - - // first time use color - // after that, use saved_color_ - static bool first_time = true; - QColor color; - if (first_time) { - ROS_ERROR_STREAM("firstTime"); - first_time = false; - color = Qt::blue; - } else { - ROS_ERROR_STREAM("anotherTime"); - color = saved_color_; - } - - // x label and x LineEdit - x_edit = new QLineEdit; - h4_layout->addWidget(new QLabel("x:")); - h4_layout->addWidget(x_edit); - - // y label and y LineEdit - y_edit = new QLineEdit; - h4_layout->addWidget(new QLabel("y:")); - h4_layout->addWidget(y_edit); - - // a label and a LineEdit - a_edit = new QLineEdit; - h4_layout->addWidget(new QLabel("a:")); - h4_layout->addWidget(a_edit); - - // ok button - h5_layout->addWidget(okButton); - connect(okButton, &QAbstractButton::clicked, this, - &ModelDialog::OkButtonClicked); - - ModelDialog::SetButtonColor(&color, color_button); - - // cancel button - h5_layout->addWidget(cancelButton); - connect(cancelButton, &QAbstractButton::clicked, this, - &ModelDialog::CancelButtonClicked); - - // add the horizontal layouts to the vertical layout - v_layout->addLayout(h1_layout); - v_layout->addLayout(h2_layout); - v_layout->addLayout(h3_layout); - v_layout->addLayout(h4_layout); - v_layout->addLayout(h5_layout); - - // set the top level layout - setLayout(v_layout); - - // delete the Dialog if the user clicks on the x to close window - this->setAttribute(Qt::WA_DeleteOnClose, true); -} - -void ModelDialog::CancelButtonClicked() { - ROS_ERROR_STREAM("Cancel clicked"); - this->close(); -} - -void ModelDialog::OkButtonClicked() { - ROS_ERROR_STREAM("Ok clicked"); - ROS_ERROR_STREAM("connect to ROS model service"); - - ModelDialog::SpawnModelClient(); -} - -void ModelDialog::SetColor() { - const QColorDialog::ColorDialogOptions options = - (QColorDialog::ShowAlphaChannel); - - const QColor color = - QColorDialog::getColor(Qt::green, this, "Select Color", options); - if (color.isValid()) { - color_button->setText(color.name()); - } - - ModelDialog::SetButtonColor(&color, color_button); - saved_color_ = color; -} - -void ModelDialog::SetButtonColor(const QColor *c, QPushButton *b) { - b->setText(c->name()); - b->setPalette(QPalette(*c)); - b->setAutoFillBackground(true); - QPalette pal = b->palette(); - pal.setColor(QPalette::Button, *c); - QString qs = "background-color:" + c->name(); - color_button->setStyleSheet(qs); -} - -void ModelDialog::SpawnModelClient() { - srv.request.name = "service_manager_test_robot"; - srv.request.ns = n_edit->text().toStdString(); - srv.request.yaml_path = path_to_model_file.toStdString(); - srv.request.pose.x = x_edit->text().toFloat(); - srv.request.pose.y = y_edit->text().toFloat(); - srv.request.pose.theta = a_edit->text().toFloat(); - - client = nh.serviceClient("spawn_model"); - - client.call(srv); -} +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name model_dialog.cpp + * @brief Main window and toolbars for flatland_viz + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "flatland_viz/model_dialog.h" + +// Initialize static variables +QColor ModelDialog::saved_color_; + +QString ModelDialog::SelectFile() { + QString fileName = + QFileDialog::getOpenFileName(this, tr("Open model file"), "", ""); + if (fileName.isEmpty()) + return fileName; + else { + QFile file(fileName); + + if (!file.open(QIODevice::ReadOnly)) { + QMessageBox::information(this, tr("Unable to open file"), + file.errorString()); + return fileName; + } + file.close(); + + return fileName; + } +} + +ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { + ROS_ERROR_STREAM("ModelDialog::ModelDialog"); + + path_to_model_file = SelectFile(); + QVBoxLayout *v_layout = new QVBoxLayout; + + // we are injecting horizontal layouts into the master vertical layout + QHBoxLayout *h1_layout = new QHBoxLayout; + QHBoxLayout *h2_layout = new QHBoxLayout; + QHBoxLayout *h3_layout = new QHBoxLayout; + QHBoxLayout *h4_layout = new QHBoxLayout; + QHBoxLayout *h5_layout = new QHBoxLayout; + + QLabel *colorLabel = new QLabel; + int frameStyle = QFrame::Sunken | QFrame::Panel; + colorLabel->setFrameStyle(frameStyle); + color_button = new QPushButton(""); + QPushButton *okButton = new QPushButton("ok"); + QPushButton *cancelButton = new QPushButton("cancel"); + connect(color_button, &QAbstractButton::clicked, this, + &ModelDialog::SetColor); + + // path label and path + QLabel *path = new QLabel; + path->setText(path_to_model_file); + h1_layout->addWidget(new QLabel("path:")); + h1_layout->addWidget(path); + + // name label and name LineEdit + n_edit = new QLineEdit; + h2_layout->addWidget(new QLabel("name:")); + h2_layout->addWidget(n_edit); + + // set the default name to the filename parsed using boost + std::string bsfn = + boost::filesystem::basename(path_to_model_file.toStdString()); + QString fn = QString::fromStdString(bsfn); + n_edit->setText(fn); + + // color label and button + h3_layout->addWidget(new QLabel("color")); + h3_layout->addWidget(color_button); + + // first time use color + // after that, use saved_color_ + static bool first_time = true; + QColor color; + if (first_time) { + ROS_ERROR_STREAM("firstTime"); + first_time = false; + color = Qt::blue; + } else { + ROS_ERROR_STREAM("anotherTime"); + color = saved_color_; + } + + // x label and x LineEdit + x_edit = new QLineEdit; + h4_layout->addWidget(new QLabel("x:")); + h4_layout->addWidget(x_edit); + + // y label and y LineEdit + y_edit = new QLineEdit; + h4_layout->addWidget(new QLabel("y:")); + h4_layout->addWidget(y_edit); + + // a label and a LineEdit + a_edit = new QLineEdit; + h4_layout->addWidget(new QLabel("a:")); + h4_layout->addWidget(a_edit); + + // ok button + h5_layout->addWidget(okButton); + connect(okButton, &QAbstractButton::clicked, this, + &ModelDialog::OkButtonClicked); + + ModelDialog::SetButtonColor(&color, color_button); + + // cancel button + h5_layout->addWidget(cancelButton); + connect(cancelButton, &QAbstractButton::clicked, this, + &ModelDialog::CancelButtonClicked); + + // add the horizontal layouts to the vertical layout + v_layout->addLayout(h1_layout); + v_layout->addLayout(h2_layout); + v_layout->addLayout(h3_layout); + v_layout->addLayout(h4_layout); + v_layout->addLayout(h5_layout); + + // set the top level layout + setLayout(v_layout); + + // delete the Dialog if the user clicks on the x to close window + this->setAttribute(Qt::WA_DeleteOnClose, true); +} + +void ModelDialog::CancelButtonClicked() { + ROS_ERROR_STREAM("Cancel clicked"); + this->close(); +} + +void ModelDialog::OkButtonClicked() { + ROS_ERROR_STREAM("Ok clicked"); + ROS_ERROR_STREAM("connect to ROS model service"); + + ModelDialog::SpawnModelClient(); +} + +void ModelDialog::SetColor() { + const QColorDialog::ColorDialogOptions options = + (QColorDialog::ShowAlphaChannel); + + const QColor color = + QColorDialog::getColor(Qt::green, this, "Select Color", options); + if (color.isValid()) { + color_button->setText(color.name()); + } + + ModelDialog::SetButtonColor(&color, color_button); + saved_color_ = color; +} + +void ModelDialog::SetButtonColor(const QColor *c, QPushButton *b) { + b->setText(c->name()); + b->setPalette(QPalette(*c)); + b->setAutoFillBackground(true); + QPalette pal = b->palette(); + pal.setColor(QPalette::Button, *c); + QString qs = "background-color:" + c->name(); + color_button->setStyleSheet(qs); +} + +void ModelDialog::SpawnModelClient() { + srv.request.name = "service_manager_test_robot"; + srv.request.ns = n_edit->text().toStdString(); + srv.request.yaml_path = path_to_model_file.toStdString(); + srv.request.pose.x = x_edit->text().toFloat(); + srv.request.pose.y = y_edit->text().toFloat(); + srv.request.pose.theta = a_edit->text().toFloat(); + + client = nh.serviceClient("spawn_model"); + + client.call(srv); +} diff --git a/flatland_viz/src/pause_sim_tool.cpp b/flatland_viz/src/pause_sim_tool.cpp index d4083fcf..cd42cc2e 100644 --- a/flatland_viz/src/pause_sim_tool.cpp +++ b/flatland_viz/src/pause_sim_tool.cpp @@ -1,29 +1,29 @@ -#include - -namespace flatland_viz { - -PauseSimTool::PauseSimTool() {} - -// Disconnect the service client when the tool's destructor is called -PauseSimTool::~PauseSimTool() { pause_service_.shutdown(); } - -// When the tool is initially loaded, connect to the pause toggle service -void PauseSimTool::onInitialize() { - pause_service_ = nh_.serviceClient("toggle_pause"); - setName("Pause/Resume"); -} - -// Every time the user presses the tool's Rviz toolbar button, call the pause -// toggle service -void PauseSimTool::activate() { - std_srvs::Empty empty; - pause_service_.call(empty); -} - -void PauseSimTool::deactivate() {} - -} // end namespace flatland_viz - -// Tell pluginlib about the tool class -#include -PLUGINLIB_EXPORT_CLASS(flatland_viz::PauseSimTool, rviz::Tool) +#include + +namespace flatland_viz { + +PauseSimTool::PauseSimTool() {} + +// Disconnect the service client when the tool's destructor is called +PauseSimTool::~PauseSimTool() { pause_service_.shutdown(); } + +// When the tool is initially loaded, connect to the pause toggle service +void PauseSimTool::onInitialize() { + pause_service_ = nh_.serviceClient("toggle_pause"); + setName("Pause/Resume"); +} + +// Every time the user presses the tool's Rviz toolbar button, call the pause +// toggle service +void PauseSimTool::activate() { + std_srvs::Empty empty; + pause_service_.call(empty); +} + +void PauseSimTool::deactivate() {} + +} // end namespace flatland_viz + +// Tell pluginlib about the tool class +#include +PLUGINLIB_EXPORT_CLASS(flatland_viz::PauseSimTool, rviz::Tool) diff --git a/flatland_viz/src/spawn_model_tool.cpp b/flatland_viz/src/spawn_model_tool.cpp index 2a1093ee..9d294192 100644 --- a/flatland_viz/src/spawn_model_tool.cpp +++ b/flatland_viz/src/spawn_model_tool.cpp @@ -1,383 +1,383 @@ -/* - * ______ __ __ __ - * /\ _ \ __ /\ \/\ \ /\ \__ - * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ - * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ - * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ - * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ - * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ - * @copyright Copyright 2018 Avidbots Corp. - * @name spawn_model_tool.cpp - * @brief Rviz compatible tool for spawning flatland model - * @author Joseph Duchesne - * @author Mike Brousseau - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Avidbots Corp. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Avidbots Corp. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include - -#include - -#include "flatland_viz/load_model_dialog.h" -#include "flatland_viz/spawn_model_tool.h" -// #include "load_model_dialog.h" -// #include "spawn_model_tool.h" - -class DialogOptionsWidget; - -namespace flatland_viz { -QString SpawnModelTool::path_to_model_file_; -QString SpawnModelTool::model_name; - -// Set the "shortcut_key_" member variable defined in the -// superclass to declare which key will activate the tool. -SpawnModelTool::SpawnModelTool() : moving_model_node_(NULL) { - shortcut_key_ = 'm'; -} - -// The destructor destroys the Ogre scene nodes for the models so they -// disappear from the 3D scene. The destructor for a Tool subclass is -// only called when the tool is removed from the toolbar with the "-" -// button. -SpawnModelTool::~SpawnModelTool() { - scene_manager_->destroySceneNode(arrow_->getSceneNode()); - scene_manager_->destroySceneNode(moving_model_node_); -} - -// onInitialize() is called by the superclass after scene_manager_ and -// context_ are set. It should be called only once per instantiation. -// This is where most one-time initialization work should be done. -// onInitialize() is called during initial instantiation of the tool -// object. At this point the tool has not been activated yet, so any -// scene objects created should be invisible or disconnected from the -// scene at this point. -// -// In this case we load a mesh object with the shape and appearance of -// the model, create an Ogre::SceneNode for the moving model, and then -// set it invisible. -void SpawnModelTool::onInitialize() { - // hide 3d model until the user clicks the span model tool button - model_state = m_hidden; - - // make an arrow to show axis of rotation - arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); - arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue - arrow_->getSceneNode()->setVisible( - false); // will only be visible during rotation phase - - // set arrow to point up (along z) - Ogre::Quaternion orientation(Ogre::Radian(M_PI), Ogre::Vector3(1, 0, 0)); - arrow_->setOrientation(orientation); - - // create an Ogre child scene node for rendering the preview outline - moving_model_node_ = - scene_manager_->getRootSceneNode()->createChildSceneNode(); - moving_model_node_->setVisible(false); - - SetMovingModelColor(Qt::green); -} - -// Activate() is called when the tool is started by the user, either -// by clicking on its button in the toolbar or by pressing its hotkey. -// -// First we set the moving model node to be visible, then we create an -// rviz::VectorProperty to show the user the position of the model. -// Unlike rviz::Display, rviz::Tool is not a subclass of -// rviz::Property, so when we want to add a tool property we need to -// get the parent container with getPropertyContainer() and add it to -// that. -// -// We wouldn't have to set current_model_property_ to be read-only, but -// if it were writable the model should really change position when the -// user edits the property. This is a fine idea, and is possible, but -// is left as an exercise for the reader. -void SpawnModelTool::activate() { - ROS_INFO_STREAM("SpawnModelTool::activate "); - - LoadModelDialog *model_dialog = new LoadModelDialog(NULL, this); - model_dialog->setModal(true); - model_dialog->show(); -} - -void SpawnModelTool::BeginPlacement() { - ROS_INFO_STREAM("SpawnModelTool::BeginPlacement"); - model_state = m_dragging; - - if (moving_model_node_) { - moving_model_node_->setVisible(true); - } -} - -// deactivate() is called when the tool is being turned off because -// another tool has been chosen. -// -// We make the moving model invisible, then delete the current model -// property. Deleting a property also removes it from its parent -// property, so that doesn't need to be done in a separate step. If -// we didn't delete it here, it would stay in the list of models when -// we switch to another tool. -void SpawnModelTool::deactivate() { - if (moving_model_node_) { - moving_model_node_->setVisible(false); - } -} - -void SpawnModelTool::SpawnModelInFlatland() { - ROS_INFO_STREAM( - "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); - flatland_msgs::SpawnModel srv; - - // model names can not have embeded period char - model_name = model_name.replace(".", "_", Qt::CaseSensitive); - - // fill in the service request - srv.request.name = model_name.toStdString(); - srv.request.ns = model_name.toStdString(); - srv.request.yaml_path = path_to_model_file_.toStdString(); - srv.request.pose.x = intersection[0]; - srv.request.pose.y = intersection[1]; - srv.request.pose.theta = initial_angle; - - client = nh.serviceClient("spawn_model"); - - // make ros service call - bool client_is_running = client.call(srv); - - if (!client_is_running) { - QMessageBox msgBox; - msgBox.setText("Error: You must have a client running."); - msgBox.exec(); - } else { - if (!srv.response.success) { - QMessageBox msgBox; - msgBox.setText(srv.response.message.c_str()); - msgBox.exec(); - } - } -} - -void SpawnModelTool::SetMovingModelColor(QColor c) { - ROS_INFO_STREAM("SpawnModelTool::SetMovingModelColor"); - - try { - Ogre::Entity *m_pEntity = - static_cast(moving_model_node_->getAttachedObject(0)); - const Ogre::MaterialPtr m_pMat = m_pEntity->getSubEntity(0)->getMaterial(); - m_pMat->getTechnique(0)->getPass(0)->setAmbient(1, 0, 0); - m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), - c.blueF(), 0); - } catch (Ogre::InvalidParametersException e) { - ROS_WARN_STREAM("Invalid preview model"); - } -} - -// processMouseEvent() is sort of the main function of a Tool, because -// mouse interactions are the point of Tools. -// -// We use the utility function rviz::getPointOnPlaneFromWindowXY() to -// see where on the ground plane the user's mouse is pointing, then -// move the moving model to that point and update the VectorProperty. -// -// If this mouse event was a left button press, we want to save the -// current model location. Therefore we make a new model at the same -// place and drop the pointer to the VectorProperty. Dropping the -// pointer means when the tool is deactivated the VectorProperty won't -// be deleted, which is what we want. -int SpawnModelTool::processMouseEvent(rviz::ViewportMouseEvent &event) { - if (!moving_model_node_) { - return Render; - } - - Ogre::Vector3 intersection2; - Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); - - if (model_state == m_dragging) { - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, - event.y, intersection)) { - moving_model_node_->setVisible(true); - moving_model_node_->setPosition(intersection); - - if (event.leftDown()) { - model_state = m_rotating; - arrow_->getSceneNode()->setVisible(true); - arrow_->setPosition(intersection); - - return Render; - } - } else { - moving_model_node_->setVisible( - false); // If the mouse is not pointing at the - // ground plane, don't show the model. - } - } - if (model_state == m_rotating) { // model_state is m_rotating - - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, - event.y, intersection2)) { - if (event.leftDown()) { - model_state = m_hidden; - arrow_->getSceneNode()->setVisible(false); - - // change Vector3 from x,y,z to x,y,angle - // z is zero since we are intersecting with the ground - intersection[2] = initial_angle; - - SpawnModelInFlatland(); - - return Render | Finished; - } - moving_model_node_->setVisible(true); - moving_model_node_->setPosition(intersection); - Ogre::Vector3 dir = intersection2 - intersection; - initial_angle = atan2(dir.y, dir.x); - Ogre::Quaternion orientation(Ogre::Radian(initial_angle), - Ogre::Vector3(0, 0, 1)); - moving_model_node_->setOrientation(orientation); - } - } - return Render; -} - -void SpawnModelTool::LoadPreview() { - // Clear the old preview, if there is one - moving_model_node_->removeAllChildren(); - lines_list_.clear(); - - // Load the bodies list into a model object - flatland_server::YamlReader reader(path_to_model_file_.toStdString()); - - try { - flatland_server::YamlReader bodies_reader = - reader.Subnode("bodies", flatland_server::YamlReader::LIST); - // Iterate each body and add to the preview - for (int i = 0; i < bodies_reader.NodeSize(); i++) { - flatland_server::YamlReader body_reader = - bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); - if (!body_reader.Get("enabled", "true")) { // skip if disabled - continue; - } - - auto pose = body_reader.GetPose("pose", flatland_server::Pose()); - - flatland_server::YamlReader footprints_node = - body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); - for (int j = 0; j < footprints_node.NodeSize(); j++) { - flatland_server::YamlReader footprint = - footprints_node.Subnode(j, flatland_server::YamlReader::MAP); - - lines_list_.push_back(std::make_shared( - context_->getSceneManager(), moving_model_node_)); - auto lines = lines_list_.back(); - lines->setColor(0.0, 1.0, 0.0, 0.75); // Green - lines->setLineWidth(0.05); - lines->setOrientation( - Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); - lines->setPosition(Ogre::Vector3(pose.x, pose.y, 0)); - - std::string type = footprint.Get("type"); - if (type == "circle") { - LoadCircleFootprint(footprint, pose); - } else if (type == "polygon") { - LoadPolygonFootprint(footprint, pose); - } else { - throw flatland_server::YAMLException("Invalid footprint \"type\""); - } - } - } - } catch (const flatland_server::YAMLException &e) { - ROS_ERROR_STREAM("Couldn't load model bodies for preview" << e.what()); - } -} - -void SpawnModelTool::LoadPolygonFootprint( - flatland_server::YamlReader &footprint, const flatland_server::Pose pose) { - auto lines = lines_list_.back(); - auto points = footprint.GetList("points", 3, - b2_maxPolygonVertices); - for (auto p : points) { - lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); - } - if (points.size() > 0) { - lines->addPoint( - Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box - } -} - -void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose) { - auto lines = lines_list_.back(); - auto center = footprint.GetVec2("center", flatland_server::Vec2()); - auto radius = footprint.Get("radius", 1.0); - for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle - lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), - center.y + radius * sin(a), 0.)); - } - lines->addPoint( - Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop -} - -void SpawnModelTool::SavePath(QString p) { - path_to_model_file_ = p; - LoadPreview(); -} - -void SpawnModelTool::SaveName(QString n) { model_name = n; } - -// At the end of every plugin class implementation, we end the -// namespace and then tell pluginlib about the class. It is important -// to do this in global scope, outside our package's namespace. - -} // end namespace flatland_viz - -#include -PLUGINLIB_EXPORT_CLASS(flatland_viz::SpawnModelTool, rviz::Tool) +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2018 Avidbots Corp. + * @name spawn_model_tool.cpp + * @brief Rviz compatible tool for spawning flatland model + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include + +#include "flatland_viz/load_model_dialog.h" +#include "flatland_viz/spawn_model_tool.h" +// #include "load_model_dialog.h" +// #include "spawn_model_tool.h" + +class DialogOptionsWidget; + +namespace flatland_viz { +QString SpawnModelTool::path_to_model_file_; +QString SpawnModelTool::model_name; + +// Set the "shortcut_key_" member variable defined in the +// superclass to declare which key will activate the tool. +SpawnModelTool::SpawnModelTool() : moving_model_node_(NULL) { + shortcut_key_ = 'm'; +} + +// The destructor destroys the Ogre scene nodes for the models so they +// disappear from the 3D scene. The destructor for a Tool subclass is +// only called when the tool is removed from the toolbar with the "-" +// button. +SpawnModelTool::~SpawnModelTool() { + scene_manager_->destroySceneNode(arrow_->getSceneNode()); + scene_manager_->destroySceneNode(moving_model_node_); +} + +// onInitialize() is called by the superclass after scene_manager_ and +// context_ are set. It should be called only once per instantiation. +// This is where most one-time initialization work should be done. +// onInitialize() is called during initial instantiation of the tool +// object. At this point the tool has not been activated yet, so any +// scene objects created should be invisible or disconnected from the +// scene at this point. +// +// In this case we load a mesh object with the shape and appearance of +// the model, create an Ogre::SceneNode for the moving model, and then +// set it invisible. +void SpawnModelTool::onInitialize() { + // hide 3d model until the user clicks the span model tool button + model_state = m_hidden; + + // make an arrow to show axis of rotation + arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); + arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue + arrow_->getSceneNode()->setVisible( + false); // will only be visible during rotation phase + + // set arrow to point up (along z) + Ogre::Quaternion orientation(Ogre::Radian(M_PI), Ogre::Vector3(1, 0, 0)); + arrow_->setOrientation(orientation); + + // create an Ogre child scene node for rendering the preview outline + moving_model_node_ = + scene_manager_->getRootSceneNode()->createChildSceneNode(); + moving_model_node_->setVisible(false); + + SetMovingModelColor(Qt::green); +} + +// Activate() is called when the tool is started by the user, either +// by clicking on its button in the toolbar or by pressing its hotkey. +// +// First we set the moving model node to be visible, then we create an +// rviz::VectorProperty to show the user the position of the model. +// Unlike rviz::Display, rviz::Tool is not a subclass of +// rviz::Property, so when we want to add a tool property we need to +// get the parent container with getPropertyContainer() and add it to +// that. +// +// We wouldn't have to set current_model_property_ to be read-only, but +// if it were writable the model should really change position when the +// user edits the property. This is a fine idea, and is possible, but +// is left as an exercise for the reader. +void SpawnModelTool::activate() { + ROS_INFO_STREAM("SpawnModelTool::activate "); + + LoadModelDialog *model_dialog = new LoadModelDialog(NULL, this); + model_dialog->setModal(true); + model_dialog->show(); +} + +void SpawnModelTool::BeginPlacement() { + ROS_INFO_STREAM("SpawnModelTool::BeginPlacement"); + model_state = m_dragging; + + if (moving_model_node_) { + moving_model_node_->setVisible(true); + } +} + +// deactivate() is called when the tool is being turned off because +// another tool has been chosen. +// +// We make the moving model invisible, then delete the current model +// property. Deleting a property also removes it from its parent +// property, so that doesn't need to be done in a separate step. If +// we didn't delete it here, it would stay in the list of models when +// we switch to another tool. +void SpawnModelTool::deactivate() { + if (moving_model_node_) { + moving_model_node_->setVisible(false); + } +} + +void SpawnModelTool::SpawnModelInFlatland() { + ROS_INFO_STREAM( + "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); + flatland_msgs::SpawnModel srv; + + // model names can not have embeded period char + model_name = model_name.replace(".", "_", Qt::CaseSensitive); + + // fill in the service request + srv.request.name = model_name.toStdString(); + srv.request.ns = model_name.toStdString(); + srv.request.yaml_path = path_to_model_file_.toStdString(); + srv.request.pose.x = intersection[0]; + srv.request.pose.y = intersection[1]; + srv.request.pose.theta = initial_angle; + + client = nh.serviceClient("spawn_model"); + + // make ros service call + bool client_is_running = client.call(srv); + + if (!client_is_running) { + QMessageBox msgBox; + msgBox.setText("Error: You must have a client running."); + msgBox.exec(); + } else { + if (!srv.response.success) { + QMessageBox msgBox; + msgBox.setText(srv.response.message.c_str()); + msgBox.exec(); + } + } +} + +void SpawnModelTool::SetMovingModelColor(QColor c) { + ROS_INFO_STREAM("SpawnModelTool::SetMovingModelColor"); + + try { + Ogre::Entity *m_pEntity = + static_cast(moving_model_node_->getAttachedObject(0)); + const Ogre::MaterialPtr m_pMat = m_pEntity->getSubEntity(0)->getMaterial(); + m_pMat->getTechnique(0)->getPass(0)->setAmbient(1, 0, 0); + m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), + c.blueF(), 0); + } catch (Ogre::InvalidParametersException e) { + ROS_WARN_STREAM("Invalid preview model"); + } +} + +// processMouseEvent() is sort of the main function of a Tool, because +// mouse interactions are the point of Tools. +// +// We use the utility function rviz::getPointOnPlaneFromWindowXY() to +// see where on the ground plane the user's mouse is pointing, then +// move the moving model to that point and update the VectorProperty. +// +// If this mouse event was a left button press, we want to save the +// current model location. Therefore we make a new model at the same +// place and drop the pointer to the VectorProperty. Dropping the +// pointer means when the tool is deactivated the VectorProperty won't +// be deleted, which is what we want. +int SpawnModelTool::processMouseEvent(rviz::ViewportMouseEvent &event) { + if (!moving_model_node_) { + return Render; + } + + Ogre::Vector3 intersection2; + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); + + if (model_state == m_dragging) { + if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + event.y, intersection)) { + moving_model_node_->setVisible(true); + moving_model_node_->setPosition(intersection); + + if (event.leftDown()) { + model_state = m_rotating; + arrow_->getSceneNode()->setVisible(true); + arrow_->setPosition(intersection); + + return Render; + } + } else { + moving_model_node_->setVisible( + false); // If the mouse is not pointing at the + // ground plane, don't show the model. + } + } + if (model_state == m_rotating) { // model_state is m_rotating + + if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + event.y, intersection2)) { + if (event.leftDown()) { + model_state = m_hidden; + arrow_->getSceneNode()->setVisible(false); + + // change Vector3 from x,y,z to x,y,angle + // z is zero since we are intersecting with the ground + intersection[2] = initial_angle; + + SpawnModelInFlatland(); + + return Render | Finished; + } + moving_model_node_->setVisible(true); + moving_model_node_->setPosition(intersection); + Ogre::Vector3 dir = intersection2 - intersection; + initial_angle = atan2(dir.y, dir.x); + Ogre::Quaternion orientation(Ogre::Radian(initial_angle), + Ogre::Vector3(0, 0, 1)); + moving_model_node_->setOrientation(orientation); + } + } + return Render; +} + +void SpawnModelTool::LoadPreview() { + // Clear the old preview, if there is one + moving_model_node_->removeAllChildren(); + lines_list_.clear(); + + // Load the bodies list into a model object + flatland_server::YamlReader reader(path_to_model_file_.toStdString()); + + try { + flatland_server::YamlReader bodies_reader = + reader.Subnode("bodies", flatland_server::YamlReader::LIST); + // Iterate each body and add to the preview + for (int i = 0; i < bodies_reader.NodeSize(); i++) { + flatland_server::YamlReader body_reader = + bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); + if (!body_reader.Get("enabled", "true")) { // skip if disabled + continue; + } + + auto pose = body_reader.GetPose("pose", flatland_server::Pose()); + + flatland_server::YamlReader footprints_node = + body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); + for (int j = 0; j < footprints_node.NodeSize(); j++) { + flatland_server::YamlReader footprint = + footprints_node.Subnode(j, flatland_server::YamlReader::MAP); + + lines_list_.push_back(std::make_shared( + context_->getSceneManager(), moving_model_node_)); + auto lines = lines_list_.back(); + lines->setColor(0.0, 1.0, 0.0, 0.75); // Green + lines->setLineWidth(0.05); + lines->setOrientation( + Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); + lines->setPosition(Ogre::Vector3(pose.x, pose.y, 0)); + + std::string type = footprint.Get("type"); + if (type == "circle") { + LoadCircleFootprint(footprint, pose); + } else if (type == "polygon") { + LoadPolygonFootprint(footprint, pose); + } else { + throw flatland_server::YAMLException("Invalid footprint \"type\""); + } + } + } + } catch (const flatland_server::YAMLException &e) { + ROS_ERROR_STREAM("Couldn't load model bodies for preview" << e.what()); + } +} + +void SpawnModelTool::LoadPolygonFootprint( + flatland_server::YamlReader &footprint, const flatland_server::Pose pose) { + auto lines = lines_list_.back(); + auto points = footprint.GetList("points", 3, + b2_maxPolygonVertices); + for (auto p : points) { + lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); + } + if (points.size() > 0) { + lines->addPoint( + Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box + } +} + +void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint, + const flatland_server::Pose pose) { + auto lines = lines_list_.back(); + auto center = footprint.GetVec2("center", flatland_server::Vec2()); + auto radius = footprint.Get("radius", 1.0); + for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle + lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), + center.y + radius * sin(a), 0.)); + } + lines->addPoint( + Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop +} + +void SpawnModelTool::SavePath(QString p) { + path_to_model_file_ = p; + LoadPreview(); +} + +void SpawnModelTool::SaveName(QString n) { model_name = n; } + +// At the end of every plugin class implementation, we end the +// namespace and then tell pluginlib about the class. It is important +// to do this in global scope, outside our package's namespace. + +} // end namespace flatland_viz + +#include +PLUGINLIB_EXPORT_CLASS(flatland_viz::SpawnModelTool, rviz::Tool) diff --git a/scripts/clang_tidy_ignore.yaml b/scripts/clang_tidy_ignore.yaml index 9c819842..dfe46473 100644 --- a/scripts/clang_tidy_ignore.yaml +++ b/scripts/clang_tidy_ignore.yaml @@ -3,3 +3,4 @@ - ".*/flatland/flatland_plugins/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" - ".*QtCore/qobject.h:235:16.*warning: Potential memory leak.*\\[clang-analyzer-cplusplus.NewDeleteLeaks\\].*" - ".*/flatland/flatland_viz/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" +- ".*/eigen3/Eigen/.*warning.*"