diff --git a/CMakeLists.txt b/CMakeLists.txt index ea63775447..9e49f43716 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ project(ignition-gazebo6 VERSION 6.0.0) # Find ignition-cmake #============================================================================ # If you get an error at this line, you need to install ignition-cmake -find_package(ignition-cmake2 2.3 REQUIRED) +find_package(ignition-cmake2 2.8.0 REQUIRED) #============================================================================ # Configure the project diff --git a/Changelog.md b/Changelog.md index acc9ced27d..901301e214 100644 --- a/Changelog.md +++ b/Changelog.md @@ -143,6 +143,49 @@ ### Ignition Gazebo 4.x.x (202x-xx-xx) +### Ignition Gazebo 4.9.1 (2021-05-24) + +1. Make halt motion act like a brake. + * [Pull Request 830](https://github.com/ignitionrobotics/ign-gazebo/pull/830) + +### Ignition Gazebo 4.9.0 (2021-05-20) + +1. Enable Focal CI. + * [Pull Request 646](https://github.com/ignitionrobotics/ign-gazebo/pull/646) + +1. [TPE] Support setting individual link velocity. + * [Pull Request 427](https://github.com/ignitionrobotics/ign-gazebo/pull/427) + +1. Don't store duplicate ComponentTypeId in ECM. + * [Pull Request 751](https://github.com/ignitionrobotics/ign-gazebo/pull/751) + +1. Fix macOS build: components::Name in benchmark. + * [Pull Request 784](https://github.com/ignitionrobotics/ign-gazebo/pull/784) + +1. Fix documentation for EntityComponentManager::EachNew. + * [Pull Request 795](https://github.com/ignitionrobotics/ign-gazebo/pull/795) + +1. Add functionalities for optical tactile plugin. + * [Pull Request 431](https://github.com/ignitionrobotics/ign-gazebo/pull/431) + +1. Visualize ContactSensorData. + * [Pull Request 234](https://github.com/ignitionrobotics/ign-gazebo/pull/234) + +1. Backport PR #763. + * [Pull Request 804](https://github.com/ignitionrobotics/ign-gazebo/pull/804) + +1. Backport PR #536. + * [Pull Request 812](https://github.com/ignitionrobotics/ign-gazebo/pull/812) + +1. Add an optional delay to the TriggeredPublisher system. + * [Pull Request 817](https://github.com/ignitionrobotics/ign-gazebo/pull/817) + +1. Remove tools/code_check and update codecov. + * [Pull Request 814](https://github.com/ignitionrobotics/ign-gazebo/pull/814) + +1. add conversion for particle scatter ratio field. + * [Pull Request 791](https://github.com/ignitionrobotics/ign-gazebo/pull/791) + ### Ignition Gazebo 4.8.0 (2021-04-22) 1. Add odometry publisher system. diff --git a/Migration.md b/Migration.md index 1b25235ba9..c393664f60 100644 --- a/Migration.md +++ b/Migration.md @@ -7,6 +7,8 @@ release will remove the deprecated code. ## Ignition Gazebo 5.x to 6.x +* Marker example has been moved to Ignition GUI. + * Some GUI plugins have been moved to Ignition GUI. Gazebo users don't need to change their configuration files, the plugins will be loaded the same way. * Grid Config diff --git a/examples/standalone/external_ecm/CMakeLists.txt b/examples/standalone/external_ecm/CMakeLists.txt new file mode 100644 index 0000000000..f03b7c51d5 --- /dev/null +++ b/examples/standalone/external_ecm/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-gazebo6 REQUIRED) + +add_executable(external_ecm external_ecm.cc) +target_link_libraries(external_ecm + ignition-gazebo6::core) diff --git a/examples/standalone/external_ecm/README.md b/examples/standalone/external_ecm/README.md new file mode 100644 index 0000000000..d65789813e --- /dev/null +++ b/examples/standalone/external_ecm/README.md @@ -0,0 +1,93 @@ +# External ECM + +Example showing how to get a snapshot of all entities and components in a +running simulation from an external program using the state message. + +## Build + +From the root of the `ign-gazebo` repository, do the following to build the example: + +~~~ +cd ign-gazebo/examples/standalone/external_ecm +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `external_ecm` executable under `build`. + +## Run + +Start a simulation, for example: + + ign gazebo shapes.sdf + +On another terminal, run the `external_ecm` executable, passing the name of the +running world you want to inspect: + + cd ign-gazebo/examples/standalone/external_ecm + ./external_ecm shapes + +You should see something like this: + +``` +$ ./external_ecm shapes + +Requesting state for world [shapes] on service [/world/shapes/state]... + +Entity [1] + - Name: shapes + - Parent: +Entity [4] + - Name: ground_plane + - Parent: shapes [1] +Entity [5] + - Name: link + - Parent: ground_plane [4] +Entity [6] + - Name: visual + - Parent: link [5] +Entity [7] + - Name: collision + - Parent: link [5] +Entity [8] + - Name: box + - Parent: shapes [1] +Entity [9] + - Name: box_link + - Parent: box [8] +Entity [10] + - Name: box_visual + - Parent: box_link [9] +Entity [11] + - Name: box_collision + - Parent: box_link [9] +Entity [12] + - Name: cylinder + - Parent: shapes [1] +Entity [13] + - Name: cylinder_link + - Parent: cylinder [12] +Entity [14] + - Name: cylinder_visual + - Parent: cylinder_link [13] +Entity [15] + - Name: cylinder_collision + - Parent: cylinder_link [13] +Entity [16] + - Name: sphere + - Parent: shapes [1] +Entity [17] + - Name: sphere_link + - Parent: sphere [16] +Entity [18] + - Name: sphere_visual + - Parent: sphere_link [17] +Entity [19] + - Name: sphere_collision + - Parent: sphere_link [17] +Entity [20] + - Name: sun + - Parent: shapes [1] +``` diff --git a/examples/standalone/external_ecm/external_ecm.cc b/examples/standalone/external_ecm/external_ecm.cc new file mode 100644 index 0000000000..6c9fa3f354 --- /dev/null +++ b/examples/standalone/external_ecm/external_ecm.cc @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include +#include + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + if (argc < 2) + { + std::cout << "Usage: `./external_ecm `" << std::endl; + return -1; + } + + // Get arguments + std::string world = argv[1]; + + // Create a transport node. + ignition::transport::Node node; + + bool executed{false}; + bool result{false}; + unsigned int timeout{5000}; + std::string service{"/world/" + world + "/state"}; + + std::cout << std::endl << "Requesting state for world [" << world + << "] on service [" << service << "]..." << std::endl << std::endl; + + // Request and block + ignition::msgs::SerializedStepMap res; + executed = node.Request(service, timeout, res, result); + + if (!executed) + { + std::cerr << std::endl << "Service call to [" << service << "] timed out" + << std::endl; + return -1; + } + + if (!result) + { + std::cerr << std::endl << "Service call to [" << service << "] failed" + << std::endl; + return -1; + } + + // Instantiate an ECM and populate with data from message + ignition::gazebo::EntityComponentManager ecm; + ecm.SetState(res.state()); + + // Print some information + ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Name *_name) -> bool + { + auto parentComp = + ecm.Component(_entity); + + std::string parentInfo; + if (parentComp) + { + auto parentNameComp = + ecm.Component( + parentComp->Data()); + + if (parentNameComp) + { + parentInfo += parentNameComp->Data() + " "; + } + parentInfo += "[" + std::to_string(parentComp->Data()) + "]"; + } + + std::cout << "Entity [" << _entity << "]" << std::endl + << " - Name: " << _name->Data() << std::endl + << " - Parent: " << parentInfo << std::endl; + + return true; + }); +} diff --git a/examples/standalone/marker/CMakeLists.txt b/examples/standalone/marker/CMakeLists.txt deleted file mode 100644 index c9bd8262fd..0000000000 --- a/examples/standalone/marker/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) - -if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) - set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - - find_package(ignition-common4 REQUIRED) - set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) - - find_package(ignition-msgs8 REQUIRED) - set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) - - add_executable(marker marker.cc) - target_link_libraries(marker - ignition-transport${IGN_TRANSPORT_VER}::core - ignition-msgs${IGN_MSGS_VER} - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} - ) -endif() diff --git a/examples/standalone/marker/README.md b/examples/standalone/marker/README.md deleted file mode 100644 index 10da135f30..0000000000 --- a/examples/standalone/marker/README.md +++ /dev/null @@ -1,22 +0,0 @@ -# Ignition Visualization Marker Example - -This example demonstrates how to create, modify, and delete visualization -markers in Ignition Gazebo. - -## Build Instructions - -From this directory: - - mkdir build - cd build - cmake .. - make - -## Execute Instructions - -Launch ign gazebo unpaused then from the build directory above: - - ./marker - -The terminal will output messages indicating visualization changes that -will occur in Ignition Gazebo's render window. diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc deleted file mode 100644 index ec57227e83..0000000000 --- a/examples/standalone/marker/marker.cc +++ /dev/null @@ -1,315 +0,0 @@ -/* - * Copyright (C) 2019 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include -#include -#include - -#include - -///////////////////////////////////////////////// -int main(int _argc, char **_argv) -{ - ignition::transport::Node node; - - // Create the marker message - ignition::msgs::Marker markerMsg; - ignition::msgs::Material matMsg; - markerMsg.set_ns("default"); - markerMsg.set_id(0); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - markerMsg.set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Blue - markerMsg.mutable_material()->mutable_ambient()->set_r(0); - markerMsg.mutable_material()->mutable_ambient()->set_g(0); - markerMsg.mutable_material()->mutable_ambient()->set_b(1); - markerMsg.mutable_material()->mutable_ambient()->set_a(1); - markerMsg.mutable_material()->mutable_diffuse()->set_r(0); - markerMsg.mutable_material()->mutable_diffuse()->set_g(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(1); - markerMsg.mutable_material()->mutable_diffuse()->set_a(1); - markerMsg.mutable_lifetime()->set_sec(2); - markerMsg.mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - - // The rest of this function adds different shapes and/or modifies shapes. - // Read the terminal statements to figure out what each node.Request - // call accomplishes. - std::cout << "Spawning a blue sphere with lifetime 2s\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(2, 2, 0, 0, 0, 0)); - node.Request("/marker", markerMsg); - std::cout << "Sleeping for 2 seconds\n"; - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "Spawning a black sphere at the origin with no lifetime\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(1); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d::Zero); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - markerMsg.mutable_lifetime()->set_sec(0); - node.Request("/marker", markerMsg); - - std::cout << "Moving the black sphere to x=0, y=1, z=1\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 1, 1, 0, 0, 0)); - node.Request("/marker", markerMsg); - - std::cout << "Shrinking the black sphere\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.2, 0.2, 0.2)); - node.Request("/marker", markerMsg); - - std::cout << "Changing the black sphere to red\n"; - markerMsg.mutable_material()->mutable_ambient()->set_r(1); - markerMsg.mutable_material()->mutable_ambient()->set_g(0); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_r(1); - markerMsg.mutable_material()->mutable_diffuse()->set_g(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - std::this_thread::sleep_for(std::chrono::seconds(4)); - node.Request("/marker", markerMsg); - - std::cout << "Adding a green ellipsoid\n"; - markerMsg.mutable_material()->mutable_ambient()->set_r(0); - markerMsg.mutable_material()->mutable_ambient()->set_g(1); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_r(0); - markerMsg.mutable_material()->mutable_diffuse()->set_g(1); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(2); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.5, 1.0, 1.5)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(2, 0, .5, 0, 0, 0)); - node.Request("/marker", markerMsg); - - std::cout << "Changing the green ellipsoid to a cylinder\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_type(ignition::msgs::Marker::CYLINDER); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.5, 0.5, 1.5)); - node.Request("/marker", markerMsg); - - std::cout << "Connecting the sphere and cylinder with a line\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(3); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 0, 0.5)); - node.Request("/marker", markerMsg); - - std::cout << "Adding a square around the origin\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(4); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_STRIP); - ignition::msgs::Set(markerMsg.mutable_point(0), - ignition::math::Vector3d(0.5, 0.5, 0.05)); - ignition::msgs::Set(markerMsg.mutable_point(1), - ignition::math::Vector3d(0.5, -0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(-0.5, -0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(-0.5, 0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0.5, 0.5, 0.05)); - node.Request("/marker", markerMsg); - - std::cout << "Adding 100 points inside the square\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(5); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::POINTS); - markerMsg.clear_point(); - for (int i = 0; i < 100; ++i) - { - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d( - ignition::math::Rand::DblUniform(-0.49, 0.49), - ignition::math::Rand::DblUniform(-0.49, 0.49), - 0.05)); - } - node.Request("/marker", markerMsg); - - std::cout << "Adding a semi-circular triangle fan\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(6); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_FAN); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 1.5, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - double radius = 2; - for (double t = 0; t <= M_PI; t+= 0.01) - { - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); - } - node.Request("/marker", markerMsg); - - std::cout << "Adding two triangles using a triangle list\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(7); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_LIST); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, -1.5, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 2, 0.05)); - - node.Request("/marker", markerMsg); - - std::cout << "Adding a rectangular triangle strip\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(8); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_STRIP); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(-2, -2, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 1, 0.05)); - - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 2, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 2, 0.05)); - - node.Request("/marker", markerMsg); - std::cout << "Adding multiple markers via /marker_array\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - - ignition::msgs::Marker_V markerMsgs; - ignition::msgs::Boolean res; - bool result; - unsigned int timeout = 5000; - - // Create first blue sphere marker - auto markerMsg1 = markerMsgs.add_marker(); - markerMsg1->set_ns("default"); - markerMsg1->set_id(0); - markerMsg1->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg1->set_type(ignition::msgs::Marker::SPHERE); - markerMsg1->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Blue - markerMsg1->mutable_material()->mutable_ambient()->set_r(0); - markerMsg1->mutable_material()->mutable_ambient()->set_g(0); - markerMsg1->mutable_material()->mutable_ambient()->set_b(1); - markerMsg1->mutable_material()->mutable_ambient()->set_a(1); - markerMsg1->mutable_material()->mutable_diffuse()->set_r(0); - markerMsg1->mutable_material()->mutable_diffuse()->set_g(0); - markerMsg1->mutable_material()->mutable_diffuse()->set_b(1); - markerMsg1->mutable_material()->mutable_diffuse()->set_a(1); - ignition::msgs::Set(markerMsg1->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg1->mutable_pose(), - ignition::math::Pose3d(3, 3, 0, 0, 0, 0)); - - // Create second red box marker - auto markerMsg2 = markerMsgs.add_marker(); - markerMsg2->set_ns("default"); - markerMsg2->set_id(0); - markerMsg2->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg2->set_type(ignition::msgs::Marker::BOX); - markerMsg2->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Red - markerMsg2->mutable_material()->mutable_ambient()->set_r(1); - markerMsg2->mutable_material()->mutable_ambient()->set_g(0); - markerMsg2->mutable_material()->mutable_ambient()->set_b(0); - markerMsg2->mutable_material()->mutable_ambient()->set_a(1); - markerMsg2->mutable_material()->mutable_diffuse()->set_r(1); - markerMsg2->mutable_material()->mutable_diffuse()->set_g(0); - markerMsg2->mutable_material()->mutable_diffuse()->set_b(0); - markerMsg2->mutable_material()->mutable_diffuse()->set_a(1); - markerMsg2->mutable_lifetime()->set_sec(2); - markerMsg2->mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg2->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg2->mutable_pose(), - ignition::math::Pose3d(3, 3, 2, 0, 0, 0)); - - // Create green capsule marker - auto markerMsg3 = markerMsgs.add_marker(); - markerMsg3->set_ns("default"); - markerMsg3->set_id(0); - markerMsg3->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg3->set_type(ignition::msgs::Marker::CAPSULE); - markerMsg3->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Green - markerMsg3->mutable_material()->mutable_ambient()->set_r(0); - markerMsg3->mutable_material()->mutable_ambient()->set_g(1); - markerMsg3->mutable_material()->mutable_ambient()->set_b(0); - markerMsg3->mutable_material()->mutable_ambient()->set_a(1); - markerMsg3->mutable_material()->mutable_diffuse()->set_r(0); - markerMsg3->mutable_material()->mutable_diffuse()->set_g(1); - markerMsg3->mutable_material()->mutable_diffuse()->set_b(0); - markerMsg3->mutable_material()->mutable_diffuse()->set_a(1); - markerMsg3->mutable_lifetime()->set_sec(2); - markerMsg3->mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg3->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg3->mutable_pose(), - ignition::math::Pose3d(3, 3, 4, 0, 0, 0)); - - // Publish the three created markers above simultaneously - node.Request("/marker_array", markerMsgs, timeout, res, result); - - std::cout << "Deleting all the markers\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - node.Request("/marker", markerMsg); -} diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index bf3d6f86f7..08a900120f 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -1,4 +1,17 @@ + @@ -38,7 +51,8 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -6 0 6 0 0.5 0 + 0.35 0.23 0.94 0 0.05 -2.53 + @@ -196,14 +210,16 @@ - true + false true - 15 - true + optical_tactile_plugin true + true + true + 15 0.01 0.001 @@ -223,7 +239,7 @@ 0 -0.7 0 0 0 3.14 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/OfficeChairBlue - + -1.5 0 0 0 0 0 https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/VendingMachine diff --git a/examples/worlds/physics_options.sdf b/examples/worlds/physics_options.sdf new file mode 100644 index 0000000000..a02c603296 --- /dev/null +++ b/examples/worlds/physics_options.sdf @@ -0,0 +1,173 @@ + + + + + + 0.001 + 1.0 + + bullet + + pgs + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + 0 0 -0.5 0 -0.52 0 + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 1.57079632679 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index e9812361c1..bb48095a06 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -6,8 +6,9 @@ Send the command: ign topic -t "/start" -m ignition.msgs.Empty -p " " The vehicle should start moving while the two boxes remain floating. When the -vehicle crosses the line on the ground, the first box should start falling. The -moment the box hits the ground, the second box should start falling. +vehicle crosses the line on the ground, the first box should start falling. +Five simulation seconds after the box hits the ground, the second box should +start falling. --> @@ -529,6 +530,7 @@ moment the box hits the ground, the second box should start falling. -7.5 + 5000 diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf new file mode 100644 index 0000000000..5db0ca25d2 --- /dev/null +++ b/examples/worlds/visualize_contacts.sdf @@ -0,0 +1,366 @@ + + + + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/visualize_contacts/control + /world/visualize_contacts/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/visualize_contacts/stats + + + + + + + 0 + 0 + 263 + 50 + floating + false + #03a9f4 + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + -0.3 0 1.15 0 0 0 + + + + + 0.3 + 0.8 + + + + + + + 0.3 + 0.8 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + collision + + + + + + + 0 0 0.325 0 0 0 + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + collision + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 5ae95dc60d..84f9e634a5 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -204,6 +204,22 @@ namespace ignition public: template ComponentTypeT *Component(const Entity _entity); + /// \brief Get a component based on a key. + /// \param[in] _key A key that uniquely identifies a component. + /// \return The component associated with the key, or nullptr if the + /// component could not be found. + public: template + const ComponentTypeT *IGN_DEPRECATED(6) Component( + const ComponentKey &_key) const; + + /// \brief Get a mutable component based on a key. + /// \param[in] _key A key that uniquely identifies a component. + /// \return The component associated with the key, or nullptr if the + /// component could not be found. + public: template + ComponentTypeT *IGN_DEPRECATED(6) Component( + const ComponentKey &_key); + /// \brief Get a mutable component assigned to an entity based on a /// component type. If the component doesn't exist, create it and /// initialize with the given default value. @@ -399,8 +415,11 @@ namespace ignition /// return false to stop subsequent calls to the callback, otherwise /// a true value should be returned. /// \tparam ComponentTypeTs All the desired component types. - /// \warning This function should not be called outside of System's - /// PreUpdate, callback. The result of call after PreUpdate is invalid + /// \warning Since entity creation occurs during PreUpdate, this function + /// should not be called in a System's PreUpdate callback (it's okay to + /// call this function in the Update callback). If you need to call this + /// function in a system's PostUpdate callback, you should use the const + /// version of this method. public: template void EachNew(typename identity void EachNew(typename identity +#include +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/components/Component.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + /// \brief Enum class that lets external users of EntityComponentStorage + /// know what type of action took place when a component was added to an + /// entity. + enum class ComponentAdditionResult + { + /// \brief The type of the component added to an entity is a type that + /// wasn't previously associated with the entity. + NEW_ADDITION = 0, + /// \brief The type of the component added to an entity was a previously + /// existing component type for that entity. + RE_ADDITION = 1, + /// \brief The component was not added to the entity because the entity + /// already has a valid instance of this component type. Instead, the data + /// for the valid instance should be modified with the new component data. + MODIFICATION = 2, + /// \brief The attempt to add the component to the entity failed, probably + /// because the entity does not exist. + FAILED_ADDITION = 3 + }; + + /// \brief A class that stores all components that have been created. This + /// class also keeps track of the entity a component belongs to. + class IGNITION_GAZEBO_VISIBLE EntityComponentStorage + { + /// \brief Add an entity to the component storage, so that newly created + /// components for this entity can be mapped to the entity. This should be + /// called whenever an entity is created. + /// \param[in] _entity The entity + /// \return True if _entity was succesfully added to the component + /// storage. False if _entity already belongs to the component storage + public: bool AddEntity(const Entity _entity); + + /// \brief Remove an entity and all of its component data. This should be + /// called whenever an entity is deleted. + /// \param[in] _entity The entity + /// \return True if _entity was in the component storage and removed. + /// False if _entity was not in the component storage + public: bool RemoveEntity(const Entity _entity); + + /// \brief Add a component to an entity. + /// \param[in] _entity The entity + /// \param[in] _component The component + /// \return The result of the attempted component addition + public: ComponentAdditionResult AddComponent(const Entity _entity, + std::unique_ptr _component); + + /// \brief Remove a component of a particular type from an entity + /// \param[in] _entity The entity + /// \return True if _entity exists and a component of type _typeId was + /// removed from it. False otherwise + public: bool RemoveComponent(const Entity _entity, + const ComponentTypeId _typeId); + + /// \brief Get a pointer to a component of a particular type that belongs + /// to an entity. This is useful for determining whether an entity has a + /// reference to a particular component type or not, but says nothing + /// about the state/validity of the component. The component's removed + /// flag should be checked to learn more about the state/validity of the + /// component. + /// \param[in] _entity The entity + /// \param[in] _typeId The type of component to be retrieved + /// \return The pointer to a component of type _typeId that belongs to + /// _entity, if it exists. Otherwise, nullptr. nullptr is also returned if + /// _entity does not exist. + /// \sa ValidComponent + private: const components::BaseComponent *Component(const Entity _entity, + const ComponentTypeId _typeId) const; + + /// \brief non-const version of the Component method + /// \sa Component + private: components::BaseComponent *Component(const Entity _entity, + const ComponentTypeId _typeId); + + /// \brief Get a pointer to a component of a particular type that belongs + /// to an entity, if the component pointer is valid. A valid component + /// pointer is one that is not nullptr and is also one that does not have + /// the removed flag set to true. + /// \param[in] _entity The entity + /// \param[in] _typeId The type of the component to retrieve + /// \return The pointer to a component, if 1) the pointer isn't nullptr, + /// and 2) the removed flag of the component pointer is false. Otherwise, + /// nullptr is returned + public: const components::BaseComponent *ValidComponent( + const Entity _entity, const ComponentTypeId _typeId) const; + + /// \brief non-const version of the ValidComponent method + /// \sa ValidComponent + public: components::BaseComponent *ValidComponent(const Entity _entity, + const ComponentTypeId _typeId); + + /// \brief A map of an entity to its components + private: std::unordered_map>> + entityComponents; + + /// \brief A map that keeps track of where each type of component is + /// located in the this->entityComponents vector. Since the + /// this->entityComponents vector is of type BaseComponent, we need to + /// keep track of which component type corresponds to a given index in + /// the vector so that we can cast the BaseComponent to this type if + /// needed. + /// + /// The key of this map is the Entity, and the value is a map of the + /// component type to the corresponding index in the + /// this->entityComponents vector (a component of a particular type is + /// only a key for the value map if a component of this type exists in + /// the this->entityComponents vector) + private: std::unordered_map> + componentTypeIndex; + }; + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition +#endif diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 67ae2cf367..d30ae00d06 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -22,6 +22,8 @@ #include #include +#include "ignition/gazebo/Entity.hh" + namespace ignition { namespace gazebo @@ -78,8 +80,8 @@ namespace ignition /// \brief A unique identifier for a component instance. The uniqueness /// of a ComponentId is scoped to the component's type. /// \sa ComponentKey. - // TODO(anyone) remove this in ign-gazebo7. This is no longer used in - // ign-gazebo6 + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. using ComponentId = int; /// \brief A unique identifier for a component type. A component type @@ -89,9 +91,10 @@ namespace ignition /// \brief A key that uniquely identifies, at the global scope, a component /// instance - // TODO(anyone) remove this in ign-gazebo7. This is no longer used in - // ign-gazebo6 - using ComponentKey = std::pair; + /// \note On version 6, the 2nd element was changed to the entity ID. + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. + using ComponentKey = std::pair; /// \brief typedef for query callbacks using EntityQueryCallback = std::function #include #include +#include #include #include #include @@ -77,10 +78,58 @@ namespace components } }; + /// \brief A base class for an object responsible for creating storages. + class StorageDescriptorBase + { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptorBase() = default; + + /// \brief Destructor + public: virtual ~StorageDescriptorBase() = default; + + /// \brief Create an instance of a storage. + /// \return Pointer to a storage. + public: virtual std::unique_ptr Create() const = 0; + }; + + /// \brief A class for an object responsible for creating storages. + /// \tparam ComponentTypeT type of component that the storage will hold. + template + class StorageDescriptor + : public StorageDescriptorBase + { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptor() = default; + + /// \brief Create an instance of a storage that holds ComponentTypeT + /// components. + /// \return Pointer to a component. + public: std::unique_ptr Create() const override + { + return std::make_unique>(); + } + }; + /// \brief A factory that generates a component based on a string type. class Factory : public ignition::common::SingletonT { + /// \brief Register a component so that the factory can create instances + /// of the component and its storage based on an ID. + /// \param[in] _type Type of component to register. + /// \param[in] _compDesc Object to manage the creation of ComponentTypeT + /// objects. + /// \param[in] _storageDesc Ignored. + /// \tparam ComponentTypeT Type of component to register. + /// \deprecated See function that doesn't accept a storage + public: template + void IGN_DEPRECATED(6) Register(const std::string &_type, + ComponentDescriptorBase *_compDesc, + StorageDescriptorBase * /*_storageDesc*/) + { + this->Register(_type, _compDesc); + } + /// \brief Register a component so that the factory can create instances /// of the component based on an ID. /// \param[in] _type Type of component to register. @@ -236,6 +285,16 @@ namespace components return comp; } + /// \brief Create a new instance of a component storage. + /// \param[in] _typeId Type of component which the storage will hold. + /// \return Always returns nullptr. + /// \deprecated Storages aren't necessary anymore. + public: std::unique_ptr IGN_DEPRECATED(6) NewStorage( + const ComponentTypeId & /*_typeId*/) + { + return nullptr; + } + /// \brief Get all the registered component types by ID. /// return Vector of component IDs. public: std::vector TypeIds() const diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh new file mode 100644 index 0000000000..2b44fa5bb8 --- /dev/null +++ b/include/ignition/gazebo/components/HaltMotion.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_ +#define IGNITION_GAZEBO_COMPONENTS_HALT_MOTION_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component used to turn off a model's joint's movement. + using HaltMotion = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.HaltMotion", + HaltMotion) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/Physics.hh b/include/ignition/gazebo/components/Physics.hh index 804059dedc..117c258367 100644 --- a/include/ignition/gazebo/components/Physics.hh +++ b/include/ignition/gazebo/components/Physics.hh @@ -17,6 +17,8 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ #define IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ +#include + #include #include @@ -48,6 +50,21 @@ namespace components serializers::PhysicsSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Physics", Physics) + + /// \brief The name of the collision detector to be used. The supported + /// options will depend on the physics engine being used. + using PhysicsCollisionDetector = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.PhysicsCollisionDetector", + PhysicsCollisionDetector) + + /// \brief The name of the solver to be used. The supported options will + /// depend on the physics engine being used. + using PhysicsSolver = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsSolver", + PhysicsSolver) } } } diff --git a/include/ignition/gazebo/detail/ComponentStorage.hh b/include/ignition/gazebo/detail/ComponentStorage.hh deleted file mode 100644 index daf003dbd5..0000000000 --- a/include/ignition/gazebo/detail/ComponentStorage.hh +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#ifndef IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGE_HH_ -#define IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGE_HH_ - -#include -#include -#include -#include - -#include "ignition/gazebo/Entity.hh" -#include "ignition/gazebo/Types.hh" -#include "ignition/gazebo/components/Component.hh" -#include "ignition/gazebo/config.hh" - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace detail -{ -/// \brief Enum class that lets external users of ComponentStorage know what -/// type of action took place when a component was added to an entity. -enum class ComponentAdditionResult -{ - /// \brief The type of the component added to an entity is a type that wasn't - /// previously associated with the entity. - NEW_ADDITION = 0, - /// \brief The type of the component added to an entity was a previously - /// existing component type for that entity. - RE_ADDITION = 1, - /// \brief The component was not added to the entity because the entity - /// already has a valid instance of this component type. Instead, the data for - /// the valid instance should be modified with the new component data. - MODIFICATION = 2, - /// \brief The attempt to add the component to the entity failed, probably - /// because the entity does not exist. - FAILED_ADDITION = 3 -}; - -/// \brief A class that stores all components that have been created. This class -/// also keeps track of the entity a component belongs to. -/// -/// \note The symbols are visible so that unit tests can be written for this -/// class. -class IGNITION_GAZEBO_VISIBLE ComponentStorage -{ - /// \brief Clear all of the entity and component data that has been tracked. - /// This "resets" the storage to its initial, empty state. - public: void Reset(); - - /// \brief Add an entity to the component storage, so that newly created - /// components for this entity can be mapped to the entity. This should be - /// called whenever an entity is created. - /// \param[in] _entity The entity - /// \return True if _entity was succesfully added to the component storage. - /// False if _entity already belongs to the component storage - public: bool AddEntity(const Entity _entity); - - /// \brief Remove an entity and all of its component data. This should be - /// called whenever an entity is deleted. - /// \param[in] _entity The entity - /// \return True if _entity was in the component storage and removed. False if - /// _entity was not in the component storage - public: bool RemoveEntity(const Entity _entity); - - /// \brief Add a component to an entity. - /// \param[in] _entity The entity - /// \param[in] _component The component - /// \return The result of the attempted component addition - public: ComponentAdditionResult AddComponent(const Entity _entity, - std::unique_ptr _component); - - /// \brief Remove a component of a particular type from an entity - /// \param[in] _entity The entity - /// \return True if _entity exists and a component of type _typeId was removed - /// from it. False otherwise - public: bool RemoveComponent(const Entity _entity, - const ComponentTypeId _typeId); - - /// \brief Get a pointer to a component of a particular type that belongs to - /// an entity. This is useful for determining whether an entity has a - /// reference to a particular component type or not, but says nothing about - /// the state/validity of the component. The component's removed flag - /// should be checked to learn more about the state/validity of the component. - /// \param[in] _entity The entity - /// \param[in] _typeId The type of component to be retrieved - /// \return The pointer to a component of type _typeId that belongs to - /// _entity, if it exists. Otherwise, nullptr. nullptr is also returned if - /// _entity does not exist. - /// \sa ValidComponent - private: const components::BaseComponent *Component(const Entity _entity, - const ComponentTypeId _typeId) const; - - /// \brief non-const version of the Component method - /// \sa Component - private: components::BaseComponent *Component(const Entity _entity, - const ComponentTypeId _typeId); - - /// \brief Get a pointer to a component of a particular type that belongs to - /// an entity, if the component pointer is valid. A valid component pointer - /// is one that is not nullptr and is also one that does not have the removed - /// flag set to true. - /// \param[in] _entity The entity - /// \param[in] _typeId The type of the component to retrieve - /// \return The pointer to a component, if 1) the pointer isn't nullptr, and - /// 2) the removed flag of the component pointer is false. Otherwise, nullptr - /// is returned - public: const components::BaseComponent *ValidComponent(const Entity _entity, - const ComponentTypeId _typeId) const; - - /// \brief non-const version of the ValidComponent method - /// \sa ValidComponent - public: components::BaseComponent *ValidComponent(const Entity _entity, - const ComponentTypeId _typeId); - - /// \brief A map of an entity to its components - private: std::unordered_map>> - entityComponents; - - /// \brief A map that keeps track of where each type of component is located - /// in the this->entityComponents vector. Since the this->entityComponents - /// vector is of type BaseComponent, we need to keep track of which component - /// type corresponds to a given index in the vector so that we can cast the - /// BaseComponent to this type if needed. - /// - /// The key of this map is the Entity, and the value is a map of the component - /// type to the corresponding index in the this->entityComponents vector - /// (a component of a particular type is only a key for the value map if a - /// component of this type exists in the this->entityComponents vector) - private: std::unordered_map> - componentTypeIndex; -}; -} // namespace detail -} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE -} // namespace gazebo -} // namespace ignition -#endif diff --git a/include/ignition/gazebo/detail/ComponentStorageBase.hh b/include/ignition/gazebo/detail/ComponentStorageBase.hh new file mode 100644 index 0000000000..cb4e0edcc2 --- /dev/null +++ b/include/ignition/gazebo/detail/ComponentStorageBase.hh @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ +#define IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ + +#include "ignition/gazebo/Export.hh" + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + // + /// \brief All component instances of the same type are stored + /// squentially in memory. This is a base class for storing components + /// of a particular type. + class IGNITION_GAZEBO_HIDDEN ComponentStorageBase + { + /// \brief Constructor + public: IGN_DEPRECATED(6) ComponentStorageBase() = default; + + /// \brief Destructor + public: virtual ~ComponentStorageBase() = default; + }; + + /// \brief Templated implementation of component storage. + template + class IGNITION_GAZEBO_HIDDEN ComponentStorage : public ComponentStorageBase + { + /// \brief Constructor + public: explicit IGN_DEPRECATED(6) ComponentStorage() + : ComponentStorageBase() + { + } + }; + } + } +} + +#endif diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index 7289679a92..d07f0bbba4 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -127,6 +127,23 @@ ComponentTypeT *EntityComponentManager::Component(const Entity _entity) this->ComponentImplementation(_entity, typeId)); } +////////////////////////////////////////////////// +template +const ComponentTypeT *EntityComponentManager::Component( + const ComponentKey &_key) const +{ + return static_cast( + this->ComponentImplementation(_key.second, _key.first)); +} + +////////////////////////////////////////////////// +template +ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key) +{ + return static_cast( + this->ComponentImplementation(_key.second, _key.first)); +} + ////////////////////////////////////////////////// template ComponentTypeT *EntityComponentManager::ComponentDefault(Entity _entity, diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index a0f82b76e9..636eca4fca 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -49,54 +49,13 @@ class IGNITION_GAZEBO_VISIBLE View : public BaseView std::tuple; /// \brief Constructor - public: View() - { - this->componentTypes = {ComponentTypeTs::typeId...}; - } + public: View(); /// \brief Documentation inherited - public: bool HasCachedComponentData(const Entity _entity) const override - { - auto cachedComps = - this->validData.find(_entity) != this->validData.end() || - this->invalidData.find(_entity) != this->invalidData.end(); - auto cachedConstComps = - this->validConstData.find(_entity) != this->validConstData.end() || - this->invalidConstData.find(_entity) != this->invalidConstData.end(); - - if (cachedComps && !cachedConstComps) - { - ignwarn << "Non-const component data is cached for entity " << _entity - << ", but const component data is not cached." << std::endl; - } - else if (cachedConstComps && !cachedComps) - { - ignwarn << "Const component data is cached for entity " << _entity - << ", but non-const component data is not cached." << std::endl; - } - - return cachedComps && cachedConstComps; - } + public: bool HasCachedComponentData(const Entity _entity) const override; /// \brief Documentation inherited - public: bool RemoveEntity(const Entity _entity) override - { - this->invalidData.erase(_entity); - this->invalidConstData.erase(_entity); - this->missingCompTracker.erase(_entity); - - if (!this->HasEntity(_entity) && !this->IsEntityMarkedForAddition(_entity)) - return false; - - this->entities.erase(_entity); - this->newEntities.erase(_entity); - this->toRemoveEntities.erase(_entity); - this->toAddEntities.erase(_entity); - this->validData.erase(_entity); - this->validConstData.erase(_entity); - - return true; - } + public: bool RemoveEntity(const Entity _entity) override; /// \brief Get an entity and its component data. It is assumed that the entity /// being requested exists in the view. @@ -104,20 +63,14 @@ class IGNITION_GAZEBO_VISIBLE View : public BaseView /// \return The entity and its component data. Const pointers to the component /// data are returned. public: ConstComponentData EntityComponentConstData( - const Entity _entity) const - { - return this->validConstData.at(_entity); - } + const Entity _entity) const; /// \brief Get an entity and its component data. It is assumed that the entity /// being requested exists in the view. /// \param[_in] _entity The entity /// \return The entity and its component data. Mutable pointers to the /// component data are returned. - public: ComponentData EntityComponentData(const Entity _entity) - { - return this->validData.at(_entity); - } + public: ComponentData EntityComponentData(const Entity _entity); /// \brief Add an entity with its component data to the view. It is assunmed /// that the entity to be added does not already exist in the view. @@ -128,13 +81,7 @@ class IGNITION_GAZEBO_VISIBLE View : public BaseView /// view or when rebuilding the view. /// \param[in] _compPtrs Const pointers to the entity's components public: void AddEntityWithConstComps(const Entity &_entity, const bool _new, - const ComponentTypeTs*... _compPtrs) - { - this->validConstData[_entity] = std::make_tuple(_entity, _compPtrs...); - this->entities.insert(_entity); - if (_new) - this->newEntities.insert(_entity); - } + const ComponentTypeTs*... _compPtrs); /// \brief Add an entity with its component data to the view. It is assunmed /// that the entity to be added does not already exist in the view. @@ -145,99 +92,18 @@ class IGNITION_GAZEBO_VISIBLE View : public BaseView /// view or when rebuilding the view. /// \param[in] _compPtrs Pointers to the entity's components public: void AddEntityWithComps(const Entity &_entity, const bool _new, - ComponentTypeTs*... _compPtrs) - { - this->validData[_entity] = std::make_tuple(_entity, _compPtrs...); - this->entities.insert(_entity); - if (_new) - this->newEntities.insert(_entity); - } + ComponentTypeTs*... _compPtrs); /// \brief Documentation inherited public: bool NotifyComponentAddition(const Entity _entity, bool _newEntity, - const ComponentTypeId _typeId) override - { - // make sure that _typeId is a type required by the view and that _entity is - // already a part of the view - if (!this->RequiresComponent(_typeId) || - !this->HasCachedComponentData(_entity)) - return false; - - // remove the newly added component type from the missing component types - // list - auto missingCompsIter = this->missingCompTracker.find(_entity); - if (missingCompsIter == this->missingCompTracker.end()) - { - // the component is already added, so nothing else needs to be done - return true; - } - missingCompsIter->second.erase(_typeId); - - // if the entity now has all components that meet the requirements of the - // view, then add the entity back to the view - if (missingCompsIter->second.empty()) - { - auto nh = this->invalidData.extract(_entity); - this->validData.insert(std::move(nh)); - auto constCompNh = this->invalidConstData.extract(_entity); - this->validConstData.insert(std::move(constCompNh)); - this->entities.insert(_entity); - if (_newEntity) - this->newEntities.insert(_entity); - this->missingCompTracker.erase(_entity); - } - - return true; - }; + const ComponentTypeId _typeId) override; /// \brief Documentation inherited public: bool NotifyComponentRemoval(const Entity _entity, - const ComponentTypeId _typeId) override - { - // make sure that _typeId is a type required by the view and that _entity is - // already a part of the view - if (!this->RequiresComponent(_typeId) || - !this->HasCachedComponentData(_entity)) - return false; - - // if the component being removed is the first component that causes _entity - // to be invalid for this view, move _entity from validData to invalidData - // since _entity should no longer be considered a part of the view - auto it = this->validData.find(_entity); - auto constCompIt = this->validConstData.find(_entity); - if (it != this->validData.end() && - constCompIt != this->validConstData.end()) - { - auto nh = this->validData.extract(it); - this->invalidData.insert(std::move(nh)); - auto constCompNh = this->validConstData.extract(constCompIt); - this->invalidConstData.insert(std::move(constCompNh)); - this->entities.erase(_entity); - this->newEntities.erase(_entity); - } - - this->missingCompTracker[_entity].insert(_typeId); - - return true; - }; + const ComponentTypeId _typeId) override; /// \brief Documentation inherited - public: void Reset() override - { - // reset all data structures in the BaseView except for componentTypes since - // the view always requires the types in componentTypes - this->entities.clear(); - this->newEntities.clear(); - this->toRemoveEntities.clear(); - this->toAddEntities.clear(); - - // reset all data structures unique to the templated view - this->validData.clear(); - this->validConstData.clear(); - this->invalidData.clear(); - this->invalidConstData.clear(); - this->missingCompTracker.clear(); - } + public: void Reset() override; /// \brief A map of entities to their component data. Since tuples are defined /// at compile time, we need seperate containers that have tuples for both @@ -278,6 +144,187 @@ class IGNITION_GAZEBO_VISIBLE View : public BaseView private: std::unordered_map> missingCompTracker; }; + +////////////////////////////////////////////////// +template +View::View() +{ + this->componentTypes = {ComponentTypeTs::typeId...}; +} + +////////////////////////////////////////////////// +template +bool View::HasCachedComponentData( + const Entity _entity) const +{ + auto cachedComps = + this->validData.find(_entity) != this->validData.end() || + this->invalidData.find(_entity) != this->invalidData.end(); + auto cachedConstComps = + this->validConstData.find(_entity) != this->validConstData.end() || + this->invalidConstData.find(_entity) != this->invalidConstData.end(); + + if (cachedComps && !cachedConstComps) + { + ignwarn << "Non-const component data is cached for entity " << _entity + << ", but const component data is not cached." << std::endl; + } + else if (cachedConstComps && !cachedComps) + { + ignwarn << "Const component data is cached for entity " << _entity + << ", but non-const component data is not cached." << std::endl; + } + + return cachedComps && cachedConstComps; +} + +////////////////////////////////////////////////// +template +bool View::RemoveEntity(const Entity _entity) +{ + this->invalidData.erase(_entity); + this->invalidConstData.erase(_entity); + this->missingCompTracker.erase(_entity); + + if (!this->HasEntity(_entity) && !this->IsEntityMarkedForAddition(_entity)) + return false; + + this->entities.erase(_entity); + this->newEntities.erase(_entity); + this->toRemoveEntities.erase(_entity); + this->toAddEntities.erase(_entity); + this->validData.erase(_entity); + this->validConstData.erase(_entity); + + return true; +} + +////////////////////////////////////////////////// +template +typename View::ConstComponentData + View::EntityComponentConstData(const Entity _entity) const +{ + return this->validConstData.at(_entity); +} + +////////////////////////////////////////////////// +template +typename View::ComponentData + View::EntityComponentData(const Entity _entity) +{ + return this->validData.at(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithConstComps(const Entity &_entity, + const bool _new, const ComponentTypeTs*... _compPtrs) +{ + this->validConstData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithComps(const Entity &_entity, + const bool _new, ComponentTypeTs*... _compPtrs) +{ + this->validData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +bool View::NotifyComponentAddition(const Entity _entity, + bool _newEntity, const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // remove the newly added component type from the missing component types + // list + auto missingCompsIter = this->missingCompTracker.find(_entity); + if (missingCompsIter == this->missingCompTracker.end()) + { + // the component is already added, so nothing else needs to be done + return true; + } + missingCompsIter->second.erase(_typeId); + + // if the entity now has all components that meet the requirements of the + // view, then add the entity back to the view + if (missingCompsIter->second.empty()) + { + auto nh = this->invalidData.extract(_entity); + this->validData.insert(std::move(nh)); + auto constCompNh = this->invalidConstData.extract(_entity); + this->validConstData.insert(std::move(constCompNh)); + this->entities.insert(_entity); + if (_newEntity) + this->newEntities.insert(_entity); + this->missingCompTracker.erase(_entity); + } + + return true; +} + +////////////////////////////////////////////////// +template +bool View::NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // if the component being removed is the first component that causes _entity + // to be invalid for this view, move _entity from validData to invalidData + // since _entity should no longer be considered a part of the view + auto it = this->validData.find(_entity); + auto constCompIt = this->validConstData.find(_entity); + if (it != this->validData.end() && + constCompIt != this->validConstData.end()) + { + auto nh = this->validData.extract(it); + this->invalidData.insert(std::move(nh)); + auto constCompNh = this->validConstData.extract(constCompIt); + this->invalidConstData.insert(std::move(constCompNh)); + this->entities.erase(_entity); + this->newEntities.erase(_entity); + } + + this->missingCompTracker[_entity].insert(_typeId); + + return true; +} + +////////////////////////////////////////////////// +template +void View::Reset() +{ + // reset all data structures in the BaseView except for componentTypes since + // the view always requires the types in componentTypes + this->entities.clear(); + this->newEntities.clear(); + this->toRemoveEntities.clear(); + this->toAddEntities.clear(); + + // reset all data structures unique to the templated view + this->validData.clear(); + this->validConstData.clear(); + this->invalidData.clear(); + this->invalidConstData.clear(); + this->missingCompTracker.clear(); +} } // namespace detail } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index fe60d03bfa..b02837a5cc 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -126,6 +126,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public : void SetRemoveSensorCb( std::function _removeSensorCb); + /// \brief View an entity as transparent + /// \param[in] _entity Entity to view as transparent + public: void ViewTransparent(const Entity &_entity); + + /// \brief View center of mass of specified entity + /// \param[in] _entity Entity to view center of mass + public: void ViewCOM(const Entity &_entity); + + /// \brief View inertia of specified entity + /// \param[in] _entity Entity to view inertia + public: void ViewInertia(const Entity &_entity); + /// \brief View wireframes of specified entity /// \param[in] _entity Entity to view wireframes public: void ViewWireframes(const Entity &_entity); @@ -158,6 +170,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Clears the set of selected entities and lowlights all of them. public: void DeselectAllEntities(); + /// \brief Helper function to get all child links of a model entity. + /// \param[in] _entity Entity to find child links + /// \return Vector of child links found for the parent entity + private: std::vector FindChildLinks(const Entity &_entity); + + /// \brief Helper function to hide wireboxes for an entity + /// \param[in] _entity Entity to hide wireboxes + private: void HideWireboxes(const Entity &_entity); + /// \brief Set whether the transform controls are currently being dragged. /// \param[in] _active True if active. public: void SetTransformActive(bool _active); diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 8ec14e44ee..9f71638ca9 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -134,6 +134,22 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); + /// \brief Create a center of mass visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parentId Parent id + /// \return Visual (center of mass) object created from the inertial + public: rendering::VisualPtr CreateCOMVisual(Entity _id, + const math::Inertiald &_inertial, Entity _parentId = 0); + + /// \brief Create an inertia visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parentId Parent id + /// \return Visual (inertia) object created from the inertial + public: rendering::VisualPtr CreateInertiaVisual(Entity _id, + const math::Inertiald &_inertial, Entity _parentId = 0); + /// \brief Create a collision visual /// \param[in] _id Unique visual id /// \param[in] _collision Collision sdf dom @@ -270,6 +286,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::NodePtr TopLevelNode( const rendering::NodePtr &_node) const; + /// \brief Updates the node to increase its transparency or reset + /// back to its original transparency value, an opaque call requires + /// a previous transparent call, otherwise, no action will be taken + /// Usually, this will be a link visual + /// \param[in] _node The node to update. + /// \param[in] _makeTransparent true if updating to increase transparency, + /// false to set back to original transparency values (make more opaque) + public: void UpdateTransparency(const rendering::NodePtr &_node, + bool _makeTransparent); + /// \internal /// \brief Pointer to private data class private: std::unique_ptr dataPtr; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7a48220efd..590ae8e9e0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -43,9 +43,9 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc BaseView.cc - ComponentStorage.cc Conversions.cc EntityComponentManager.cc + EntityComponentStorage.cc LevelManager.cc Link.cc Model.cc @@ -66,24 +66,24 @@ set (gtest_sources ${gtest_sources} Barrier_TEST.cc BaseView_TEST.cc - Component_TEST.cc ComponentFactory_TEST.cc - ComponentStorage_TEST.cc + Component_TEST.cc Conversions_TEST.cc EntityComponentManager_TEST.cc + EntityComponentStorage_TEST.cc EventManager_TEST.cc - ign_TEST.cc Link_TEST.cc Model_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc - Server_TEST.cc ServerConfig_TEST.cc + Server_TEST.cc SimulationRunner_TEST.cc - System_TEST.cc SystemLoader_TEST.cc + System_TEST.cc Util_TEST.cc World_TEST.cc + ign_TEST.cc network/NetworkConfig_TEST.cc network/PeerTracker_TEST.cc network/NetworkManager_TEST.cc diff --git a/src/Conversions.cc b/src/Conversions.cc index 322b912221..a71ef0aafb 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1504,7 +1504,7 @@ msgs::ParticleEmitter ignition::gazebo::convert(const sdf::ParticleEmitter &_in) { msgs::ParticleEmitter out; out.set_name(_in.Name()); - switch(_in.Type()) + switch (_in.Type()) { default: case sdf::ParticleEmitterType::POINT: @@ -1560,6 +1560,13 @@ msgs::ParticleEmitter ignition::gazebo::convert(const sdf::ParticleEmitter &_in) header->add_value(_in.Topic()); } + // todo(anyone) Use particle_scatter_ratio in particle_emitter.proto from + // Fortress on. + auto header = out.mutable_header()->add_data(); + header->set_key("particle_scatter_ratio"); + std::string *value = header->add_value(); + *value = std::to_string(_in.ScatterRatio()); + return out; } @@ -1570,7 +1577,7 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) { sdf::ParticleEmitter out; out.SetName(_in.name()); - switch(_in.type()) + switch (_in.type()) { default: case msgs::ParticleEmitter::POINT: @@ -1619,6 +1626,10 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) { out.SetTopic(data.value(0)); } + else if (data.key() == "particle_scatter_ratio" && data.value_size() > 0) + { + out.SetScatterRatio(std::stof(data.value(0))); + } } return out; diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index e957851bda..ce84b9bb98 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -962,6 +962,7 @@ TEST(Conversions, ParticleEmitter) emitter.SetColorRangeImage("range_image"); emitter.SetTopic("my_topic"); emitter.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); + emitter.SetScatterRatio(0.9f); sdf::Material material; sdf::Pbr pbr; @@ -996,6 +997,10 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ("topic", header.key()); EXPECT_EQ("my_topic", header.value(0)); + auto headerScatterRatio = emitterMsg.header().data(1); + EXPECT_EQ("particle_scatter_ratio", headerScatterRatio.key()); + EXPECT_FLOAT_EQ(0.9f, std::stof(headerScatterRatio.value(0))); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), msgs::Convert(emitterMsg.pose())); auto pbrMsg = emitterMsg.material().pbr(); @@ -1020,4 +1025,5 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(emitter2.ColorRangeImage(), emitter.ColorRangeImage()); EXPECT_EQ(emitter2.Topic(), emitter.Topic()); EXPECT_EQ(emitter2.RawPose(), emitter.RawPose()); + EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio()); } diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 9a54bac74a..589b78fdc4 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -15,6 +15,8 @@ * */ +#include "ignition/gazebo/EntityComponentManager.hh" + #include #include #include @@ -25,10 +27,10 @@ #include #include + +#include "ignition/gazebo/EntityComponentStorage.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" -#include "ignition/gazebo/detail/ComponentStorage.hh" -#include "ignition/gazebo/EntityComponentManager.hh" using namespace ignition; using namespace gazebo; @@ -78,7 +80,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief A class that stores all components and maps entities to their /// component types - public: detail::ComponentStorage componentStorage; + public: EntityComponentStorage entityCompStorage; /// \brief All component types that have ever been created. public: std::unordered_set createdCompTypes; @@ -211,7 +213,7 @@ Entity EntityComponentManagerPrivate::CreateEntityImplementation(Entity _entity) // Reset descendants cache this->descendantCache.clear(); - if (!this->componentStorage.AddEntity(_entity)) + if (!this->entityCompStorage.AddEntity(_entity)) { ignwarn << "Attempted to add entity [" << _entity << "] to component storage, but this entity is already in component " @@ -307,7 +309,8 @@ void EntityComponentManager::ProcessRemoveEntityRequests() this->dataPtr->toRemoveEntities.clear(); this->dataPtr->entityComponentsDirty = true; - this->dataPtr->componentStorage.Reset(); + // reset the entity component storage + this->dataPtr->entityCompStorage = EntityComponentStorage(); // All views are now invalid. this->dataPtr->views.clear(); @@ -329,7 +332,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests() // Remove the components, if any. if (entityIter != this->dataPtr->entityComponents.end()) { - this->dataPtr->componentStorage.RemoveEntity(entity); + this->dataPtr->entityCompStorage.RemoveEntity(entity); // Remove the entry in the entityComponent map this->dataPtr->entityComponents.erase(entity); @@ -379,7 +382,7 @@ bool EntityComponentManager::RemoveComponent( } auto removedComp = - this->dataPtr->componentStorage.RemoveComponent(_entity, _typeId); + this->dataPtr->entityCompStorage.RemoveComponent(_entity, _typeId); if (removedComp) { // update views to reflect the component removal @@ -402,9 +405,6 @@ bool EntityComponentManager::RemoveComponent( bool EntityComponentManager::RemoveComponent( const Entity _entity, const ComponentKey &_key) { - ignwarn << "EntityComponentManager::RemoveComponent is deprecated. Please " - << "use the RemoveComponent method with a ComponentTypeId parameter type " - << "instead." << std::endl; return this->RemoveComponent(_entity, _key.first); } @@ -412,9 +412,6 @@ bool EntityComponentManager::RemoveComponent( bool EntityComponentManager::EntityHasComponent(const Entity _entity, const ComponentKey &_key) const { - ignwarn << "EntityComponentManager::EntityHasComponent is deprecated. Please " - << "use EntityComponentManager::EntityHasComponentType instead." - << std::endl; if (!this->HasEntity(_entity)) return false; auto &compSet = this->dataPtr->entityComponents[_entity]; @@ -597,14 +594,14 @@ bool EntityComponentManager::CreateComponentImplementation( auto newComp = components::Factory::Instance()->New(_componentTypeId, _data); auto compAddResult = - this->dataPtr->componentStorage.AddComponent(_entity, std::move(newComp)); + this->dataPtr->entityCompStorage.AddComponent(_entity, std::move(newComp)); switch (compAddResult) { case detail::ComponentAdditionResult::FAILED_ADDITION: ignerr << "Attempt to create a component of type [" << _componentTypeId << "] attached to entity [" << _entity << "] failed.\n"; return false; - case detail::ComponentAdditionResult::NEW_ADDITION: + case ComponentAdditionResult::NEW_ADDITION: updateData = false; for (auto &viewPair : this->dataPtr->views) { @@ -613,12 +610,12 @@ bool EntityComponentManager::CreateComponentImplementation( view->MarkEntityToAdd(_entity, this->IsNewEntity(_entity)); } break; - case detail::ComponentAdditionResult::RE_ADDITION: + case ComponentAdditionResult::RE_ADDITION: for (auto &viewPair : this->dataPtr->views) viewPair.second->NotifyComponentAddition(_entity, this->IsNewEntity(_entity), _componentTypeId); break; - case detail::ComponentAdditionResult::MODIFICATION: + case ComponentAdditionResult::MODIFICATION: break; default: ignerr << "Undefined behavior occurred when creating a component of " @@ -665,14 +662,14 @@ const components::BaseComponent const Entity _entity, const ComponentTypeId _type) const { IGN_PROFILE("EntityComponentManager::ComponentImplementation"); - return this->dataPtr->componentStorage.ValidComponent(_entity, _type); + return this->dataPtr->entityCompStorage.ValidComponent(_entity, _type); } ///////////////////////////////////////////////// components::BaseComponent *EntityComponentManager::ComponentImplementation( const Entity _entity, const ComponentTypeId _type) { - return this->dataPtr->componentStorage.ValidComponent(_entity, _type); + return this->dataPtr->entityCompStorage.ValidComponent(_entity, _type); } ///////////////////////////////////////////////// @@ -1274,13 +1271,6 @@ void EntityComponentManager::SetState( { const auto &compMsg = compIter.second; - // Skip if component not set. Note that this will also skip components - // setting an empty value. - if (compMsg.component().empty()) - { - continue; - } - uint64_t type = compMsg.type(); // Components which haven't been registered in this process, such as 3rd diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 95cb38a785..d0ab939044 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Pose.hh" @@ -2582,6 +2583,81 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } } +///////////////////////////////////////////////// +// Check that some widely used deprecated APIs still work +TEST_P(EntityComponentManagerFixture, Deprecated) +{ + IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + // Fail to create component for inexistent entity + EXPECT_EQ(nullptr, manager.CreateComponent(789, + IntComponent(123))); + + // Create some entities + auto eInt = manager.CreateEntity(); + auto eDouble = manager.CreateEntity(); + auto eIntDouble = manager.CreateEntity(); + EXPECT_EQ(3u, manager.EntityCount()); + + // Add components and keep their unique ComponentKeys + EXPECT_NE(nullptr, manager.CreateComponent(eInt, + IntComponent(123))); + ComponentKey cIntEInt = {IntComponent::typeId, eInt}; + + EXPECT_NE(nullptr, manager.CreateComponent(eDouble, + DoubleComponent(0.123))); + ComponentKey cDoubleEDouble = {DoubleComponent::typeId, eDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + IntComponent(456))); + ComponentKey cIntEIntDouble = {IntComponent::typeId, eIntDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + DoubleComponent(0.456))); + ComponentKey cDoubleEIntDouble = {DoubleComponent::typeId, eIntDouble}; + + // Check entities have the components + EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); + EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); + EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); + auto types = manager.ComponentTypes(eIntDouble); + EXPECT_NE(types.end(), types.find(IntComponent::typeId)); + EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); + + // Remove component by key + EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); + EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + + // Remove component by type id + auto typeDouble = DoubleComponent::typeId; + + EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); + + // Remove component by type + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); + + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); + + IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/ComponentStorage.cc b/src/EntityComponentStorage.cc similarity index 85% rename from src/ComponentStorage.cc rename to src/EntityComponentStorage.cc index 39c77e1297..919e629753 100644 --- a/src/ComponentStorage.cc +++ b/src/EntityComponentStorage.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include "ignition/gazebo/detail/ComponentStorage.hh" +#include "ignition/gazebo/EntityComponentStorage.hh" #include #include @@ -27,17 +27,9 @@ using namespace ignition; using namespace gazebo; -using namespace detail; ////////////////////////////////////////////////// -void ComponentStorage::Reset() -{ - this->entityComponents.clear(); - this->componentTypeIndex.clear(); -} - -////////////////////////////////////////////////// -bool ComponentStorage::AddEntity(const Entity _entity) +bool EntityComponentStorage::AddEntity(const Entity _entity) { const auto [it, success] = this->entityComponents.insert({_entity, std::vector>()}); @@ -51,7 +43,7 @@ bool ComponentStorage::AddEntity(const Entity _entity) } ////////////////////////////////////////////////// -bool ComponentStorage::RemoveEntity(const Entity _entity) +bool EntityComponentStorage::RemoveEntity(const Entity _entity) { const auto removedComponents = this->entityComponents.erase(_entity); const auto removedTypeIdxMap = this->componentTypeIndex.erase(_entity); @@ -59,7 +51,7 @@ bool ComponentStorage::RemoveEntity(const Entity _entity) } ////////////////////////////////////////////////// -ComponentAdditionResult ComponentStorage::AddComponent( +ComponentAdditionResult EntityComponentStorage::AddComponent( const Entity _entity, std::unique_ptr _component) { @@ -101,7 +93,7 @@ ComponentAdditionResult ComponentStorage::AddComponent( } ////////////////////////////////////////////////// -bool ComponentStorage::RemoveComponent(const Entity _entity, +bool EntityComponentStorage::RemoveComponent(const Entity _entity, const ComponentTypeId _typeId) { auto compPtr = this->Component(_entity, _typeId); @@ -116,7 +108,7 @@ bool ComponentStorage::RemoveComponent(const Entity _entity, } ////////////////////////////////////////////////// -const components::BaseComponent *ComponentStorage::Component( +const components::BaseComponent *EntityComponentStorage::Component( const Entity _entity, const ComponentTypeId _typeId) const { @@ -144,15 +136,16 @@ const components::BaseComponent *ComponentStorage::Component( } ////////////////////////////////////////////////// -components::BaseComponent *ComponentStorage::Component(const Entity _entity, - const ComponentTypeId _typeId) +components::BaseComponent *EntityComponentStorage::Component( + const Entity _entity, const ComponentTypeId _typeId) { return const_cast( - static_cast(*this).Component(_entity, _typeId)); + static_cast(*this).Component(_entity, + _typeId)); } ////////////////////////////////////////////////// -const components::BaseComponent *ComponentStorage::ValidComponent( +const components::BaseComponent *EntityComponentStorage::ValidComponent( const Entity _entity, const ComponentTypeId _typeId) const { auto compPtr = this->Component(_entity, _typeId); @@ -162,7 +155,7 @@ const components::BaseComponent *ComponentStorage::ValidComponent( } ////////////////////////////////////////////////// -components::BaseComponent *ComponentStorage::ValidComponent( +components::BaseComponent *EntityComponentStorage::ValidComponent( const Entity _entity, const ComponentTypeId _typeId) { auto compPtr = this->Component(_entity, _typeId); diff --git a/src/ComponentStorage_TEST.cc b/src/EntityComponentStorage_TEST.cc similarity index 75% rename from src/ComponentStorage_TEST.cc rename to src/EntityComponentStorage_TEST.cc index 49f125f031..ca7da86526 100644 --- a/src/ComponentStorage_TEST.cc +++ b/src/EntityComponentStorage_TEST.cc @@ -21,12 +21,12 @@ #include #include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentStorage.hh" #include "ignition/gazebo/Types.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Pose.hh" -#include "ignition/gazebo/detail/ComponentStorage.hh" using namespace ignition; using namespace gazebo; @@ -42,7 +42,7 @@ const Entity entity3 = 3; const Entity entity4 = 4; ///////////////////////////////////////////////// -class ComponentStorageTest : public ::testing::Test +class EntityComponentStorageTest : public ::testing::Test { // Documentation inherited protected: void SetUp() override @@ -56,7 +56,7 @@ class ComponentStorageTest : public ::testing::Test /// \param[in] _typeId The type ID of _component /// \param[in] _component The component, which belongs to _entity /// \return The result of the component addition - public: detail::ComponentAdditionResult AddComponent(const Entity _entity, + public: ComponentAdditionResult AddComponent(const Entity _entity, const ComponentTypeId _typeId, const components::BaseComponent *_component) { @@ -65,8 +65,8 @@ class ComponentStorageTest : public ::testing::Test return this->storage.AddComponent(_entity, std::move(compPtr)); } - /// \brief Helper function that uses ComponentStorage::ValidComponent to get - /// a component of a particular type that belongs to an entity. + /// \brief Helper function that uses EntityComponentStorage::ValidComponent to + /// get a component of a particular type that belongs to an entity. /// \param[in] _entity The entity /// \return A pointer to the component of the templated type. If no such /// component exists, nullptr is returned @@ -87,7 +87,7 @@ class ComponentStorageTest : public ::testing::Test return static_cast(baseComp); } - public: detail::ComponentStorage storage; + public: EntityComponentStorage storage; public: const Entity e1{1}; @@ -95,7 +95,7 @@ class ComponentStorageTest : public ::testing::Test }; ///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, AddEntity) +TEST_F(EntityComponentStorageTest, AddEntity) { // Entities were already added to the storage in the test fixture's SetUp // method. So, try to add entities that are already in the storage. @@ -109,7 +109,7 @@ TEST_F(ComponentStorageTest, AddEntity) } ///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, RemoveEntity) +TEST_F(EntityComponentStorageTest, RemoveEntity) { // Try to remove entities that aren't in the storage EXPECT_FALSE(this->storage.RemoveEntity(3)); @@ -125,12 +125,12 @@ TEST_F(ComponentStorageTest, RemoveEntity) } ///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, AddComponent) +TEST_F(EntityComponentStorageTest, AddComponent) { // Add components to entities in the storage - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e1, components::Pose::typeId, &poseComp)); - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e2, components::LinearVelocity::typeId, &linVelComp)); @@ -149,9 +149,9 @@ TEST_F(ComponentStorageTest, AddComponent) // remove call should fail) EXPECT_FALSE(this->storage.RemoveEntity(entity3)); EXPECT_FALSE(this->storage.RemoveEntity(entity4)); - EXPECT_EQ(detail::ComponentAdditionResult::FAILED_ADDITION, + EXPECT_EQ(ComponentAdditionResult::FAILED_ADDITION, this->AddComponent(entity3, components::Pose::typeId, &poseComp)); - EXPECT_EQ(detail::ComponentAdditionResult::FAILED_ADDITION, + EXPECT_EQ(ComponentAdditionResult::FAILED_ADDITION, this->AddComponent(entity4, components::LinearVelocity::typeId, &linVelComp)); EXPECT_EQ(nullptr, this->Component(entity3)); @@ -159,16 +159,17 @@ TEST_F(ComponentStorageTest, AddComponent) // Add components to entities that already have a component of the same type // (this has the same effect of modifying an existing component) - EXPECT_EQ(detail::ComponentAdditionResult::MODIFICATION, + EXPECT_EQ(ComponentAdditionResult::MODIFICATION, this->AddComponent(this->e1, components::Pose::typeId, &poseComp2)); - EXPECT_EQ(detail::ComponentAdditionResult::MODIFICATION, + EXPECT_EQ(ComponentAdditionResult::MODIFICATION, this->AddComponent(this->e2, components::LinearVelocity::typeId, &linVelComp2)); // We can't check if the modification actually took place since this requires - // functionality beyond the ComponentStorage API (see the comments in the - // ComponentStorage::AddComponent method definition for more details), but we - // can at least check that the components still exist after modification + // functionality beyond the EntityComponentStorage API (see the comments in + // the EntityComponentStorage::AddComponent method definition for more + // details), but we can at least check that the components still exist after + // modification ASSERT_NE(nullptr, this->Component(this->e1)); ASSERT_NE(nullptr, this->Component(this->e2)); @@ -177,7 +178,7 @@ TEST_F(ComponentStorageTest, AddComponent) EXPECT_TRUE(this->storage.RemoveComponent(this->e1, components::Pose::typeId)); EXPECT_EQ(nullptr, this->Component(this->e1)); - EXPECT_EQ(detail::ComponentAdditionResult::RE_ADDITION, + EXPECT_EQ(ComponentAdditionResult::RE_ADDITION, this->AddComponent(this->e1, components::Pose::typeId, &poseComp)); storagePoseComp = this->Component(this->e1); ASSERT_NE(nullptr, storagePoseComp); @@ -186,7 +187,7 @@ TEST_F(ComponentStorageTest, AddComponent) EXPECT_TRUE(this->storage.RemoveComponent(this->e2, components::LinearVelocity::typeId)); EXPECT_EQ(nullptr, this->Component(this->e2)); - EXPECT_EQ(detail::ComponentAdditionResult::RE_ADDITION, + EXPECT_EQ(ComponentAdditionResult::RE_ADDITION, this->AddComponent(this->e2, components::LinearVelocity::typeId, &linVelComp)); storageLinVelComp = this->Component(this->e2); @@ -195,13 +196,13 @@ TEST_F(ComponentStorageTest, AddComponent) } ///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, RemoveComponent) +TEST_F(EntityComponentStorageTest, RemoveComponent) { // Add components to entities - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e1, poseComp.TypeId(), &poseComp)); EXPECT_NE(nullptr, this->Component(this->e1)); - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e2, linVelComp.TypeId(), &linVelComp)); EXPECT_NE(nullptr, this->Component(this->e2)); @@ -231,10 +232,10 @@ TEST_F(ComponentStorageTest, RemoveComponent) } ///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, ValidComponent) +TEST_F(EntityComponentStorageTest, ValidComponent) { // Attach a component to an entity - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e1, poseComp.TypeId(), &poseComp)); // Get a component that is attached to an entity @@ -260,12 +261,12 @@ TEST_F(ComponentStorageTest, ValidComponent) ///////////////////////////////////////////////// // Similar to the ValidComponent test, but this test covers the const version of -// the ComponentStorage::ValidComponent method (the ValidComponent test covered -// the non-const version of the ComponentStorage::ValidComponent) -TEST_F(ComponentStorageTest, ValidComponentConst) +// the EntityComponentStorage::ValidComponent method (the ValidComponent test +// covered the non-const version of the EntityComponentStorage::ValidComponent) +TEST_F(EntityComponentStorageTest, ValidComponentConst) { // Attach a component to an entity - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(this->e1, poseComp.TypeId(), &poseComp)); // Get a component that is attached to an entity @@ -283,43 +284,3 @@ TEST_F(ComponentStorageTest, ValidComponentConst) components::Pose::typeId)); ASSERT_EQ(nullptr, this->ConstComponent(this->e1)); } - -///////////////////////////////////////////////// -TEST_F(ComponentStorageTest, Reset) -{ - // Add components to entities in the storage - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, - this->AddComponent(this->e1, poseComp.TypeId(), &poseComp)); - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, - this->AddComponent(this->e2, linVelComp.TypeId(), &linVelComp)); - - // Make sure that the storage has entities and components - EXPECT_FALSE(this->storage.AddEntity(this->e1)); - EXPECT_FALSE(this->storage.AddEntity(this->e2)); - EXPECT_NE(nullptr, this->storage.ValidComponent(this->e1, - components::Pose::typeId)); - EXPECT_NE(nullptr, this->storage.ValidComponent(this->e2, - components::LinearVelocity::typeId)); - - // Reset the storage - this->storage.Reset(); - - // Make sure the storage has no entities or components - EXPECT_FALSE(this->storage.RemoveEntity(this->e1)); - EXPECT_EQ(nullptr, this->storage.ValidComponent(this->e1, - components::Pose::typeId)); - EXPECT_FALSE(this->storage.RemoveEntity(this->e2)); - EXPECT_EQ(nullptr, this->storage.ValidComponent(this->e2, - components::LinearVelocity::typeId)); - - // We should be able to re-add the entities that were removed - EXPECT_TRUE(this->storage.AddEntity(this->e1)); - EXPECT_TRUE(this->storage.AddEntity(this->e2)); - - // The entities that were re-added shouldn't have any components associated - // with them since resetting the storage removed all components - EXPECT_EQ(nullptr, this->storage.ValidComponent(this->e1, - components::Pose::typeId)); - EXPECT_EQ(nullptr, this->storage.ValidComponent(this->e2, - components::LinearVelocity::typeId)); -} diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 74e2f71020..1b4d2f39d2 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -111,6 +111,31 @@ void LevelManager::ReadLevelPerformerInfo() this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::Physics(*physics)); + // Populate physics options that aren't accessible outside the Element() + // See https://github.com/osrf/sdformat/issues/508 + if (physics->Element() && physics->Element()->HasElement("dart")) + { + auto dartElem = physics->Element()->GetElement("dart"); + + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("collision_detector"); + + this->runner->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsCollisionDetector(collisionDetector)); + } + if (dartElem->HasElement("solver") && + dartElem->GetElement("solver")->HasElement("solver_type")) + { + auto solver = + dartElem->GetElement("solver")->Get("solver_type"); + + this->runner->entityCompMgr.CreateComponent(worldEntity, + components::PhysicsSolver(solver)); + } + } + this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::MagneticField(this->runner->sdfWorld->MagneticField())); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d7c98189c6..40b1be0ae2 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -280,6 +280,31 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->ecm->CreateComponent(worldEntity, components::Physics(*physics)); + // Populate physics options that aren't accessible outside the Element() + // See https://github.com/osrf/sdformat/issues/508 + if (physics->Element() && physics->Element()->HasElement("dart")) + { + auto dartElem = physics->Element()->GetElement("dart"); + + if (dartElem->HasElement("collision_detector")) + { + auto collisionDetector = + dartElem->Get("collision_detector"); + + this->dataPtr->ecm->CreateComponent(worldEntity, + components::PhysicsCollisionDetector(collisionDetector)); + } + if (dartElem->HasElement("solver") && + dartElem->GetElement("solver")->HasElement("solver_type")) + { + auto solver = + dartElem->GetElement("solver")->Get("solver_type"); + + this->dataPtr->ecm->CreateComponent(worldEntity, + components::PhysicsSolver(solver)); + } + } + // MagneticField this->dataPtr->ecm->CreateComponent(worldEntity, components::MagneticField(_world->MagneticField())); diff --git a/src/gui/gui.config b/src/gui/gui.config index 072f631f63..137c55e648 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -21,6 +21,7 @@ + true diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index eef52f90ff..fa8b814429 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -131,4 +131,5 @@ add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) +add_subdirectory(visualize_contacts) add_subdirectory(visualize_lidar) diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index ee106c9f7c..c605edd27b 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -25,25 +25,27 @@ #include #include +#include #include #include #include -#include #include -#include -#include #include #include +#include #include #include -#include #include +#include #include -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/Name.hh" +#include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + #include "AlignTool.hh" namespace ignition::gazebo @@ -89,6 +91,9 @@ namespace ignition::gazebo /// \brief The map of the original transparency values for the nodes. public: std::map originalTransparency; + + /// \brief Pointer to the scene. + public: rendering::ScenePtr scene{nullptr}; }; } @@ -342,52 +347,9 @@ void AlignTool::Align() if (this->dataPtr->currentState == AlignState::NONE) return; - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - { - ignerr << "Internal error: engine should be loaded at this point." - << std::endl; - return; - } - - // Assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - ignwarn << "Found more than one loaded engine " - "- using " << engineName << "." << std::endl; - } - auto engine = rendering::engine(engineName); - - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Align tool plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - { - ignerr<< "Internal error: no scenes are available with the loaded engine." - << std::endl; - return; - } - // assume there is only one scene // load scene - auto scene = engine->SceneByIndex(0); - - if (!scene) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized() || scene->VisualCount() == 0) - { - ignerr << "Internal error: scene is either not initialized " - "or there are no visuals within it." << std::endl; - return; - } + if (!this->dataPtr->scene) + this->dataPtr->scene = rendering::sceneFromFirstRenderEngine(); // Get current list of selected entities std::vector selectedList; @@ -395,9 +357,10 @@ void AlignTool::Align() for (const auto &entityId : this->dataPtr->selectedEntities) { - for (auto i = 0u; i < scene->VisualCount(); ++i) + for (auto i = 0u; i < this->dataPtr->scene->VisualCount(); ++i) { - ignition::rendering::VisualPtr vis = scene->VisualByIndex(i); + ignition::rendering::VisualPtr vis = + this->dataPtr->scene->VisualByIndex(i); if (!vis) continue; @@ -405,7 +368,7 @@ void AlignTool::Align() static_cast(entityId)) { // Check here to see if visual is top level or not, continue if not - rendering::VisualPtr topLevelVis = this->TopLevelVisual(scene, vis); + auto topLevelVis = this->TopLevelVisual(this->dataPtr->scene, vis); if (topLevelVis != vis) continue; @@ -459,7 +422,8 @@ void AlignTool::Align() ignition::math::Vector3d max = box.Max(); // Check here to see if visual is top level or not, continue if not - rendering::VisualPtr topLevelVis = this->TopLevelVisual(scene, vis); + rendering::VisualPtr topLevelVis = this->TopLevelVisual( + this->dataPtr->scene, vis); if (topLevelVis != vis) continue; diff --git a/src/gui/plugins/align_tool/AlignTool.hh b/src/gui/plugins/align_tool/AlignTool.hh index f426da7208..b18e158564 100644 --- a/src/gui/plugins/align_tool/AlignTool.hh +++ b/src/gui/plugins/align_tool/AlignTool.hh @@ -20,8 +20,8 @@ #include -#include #include +#include #include namespace ignition diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 30467ea5f2..3dd0258575 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -49,8 +49,11 @@ #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerAffinity.hh" #include "ignition/gazebo/components/Physics.hh" +#include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" +#include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" @@ -463,6 +466,12 @@ void ComponentInspector::Update(const UpdateInfo &, continue; } + if (typeId == components::Light::typeId) + { + this->SetType("light"); + continue; + } + if (typeId == components::Actor::typeId) { this->SetType("actor"); @@ -631,12 +640,40 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::PhysicsCollisionDetector::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::PhysicsSolver::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); if (comp) setData(item, comp->Data()); } + else if (typeId == components::RenderEngineGuiPlugin::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::RenderEngineServerPlugin::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Static::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 911093eaa8..4d0e0af452 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -47,6 +47,15 @@ namespace ignition::gazebo /// \brief Remove service name public: std::string removeService; + /// \brief View as transparent service name + public: std::string viewTransparentService; + + /// \brief View center of mass service name + public: std::string viewCOMService; + + /// \brief View inertia service name + public: std::string viewInertiaService; + /// \brief View wireframes service name public: std::string viewWireframesService; @@ -82,6 +91,15 @@ EntityContextMenu::EntityContextMenu() // For remove service requests this->dataPtr->removeService = "/world/default/remove"; + // For view transparent service requests + this->dataPtr->viewTransparentService = "/gui/view/transparent"; + + // For view center of mass service requests + this->dataPtr->viewCOMService = "/gui/view/com"; + + // For view inertia service requests + this->dataPtr->viewInertiaService = "/gui/view/inertia"; + // For view wireframes service requests this->dataPtr->viewWireframesService = "/gui/view/wireframes"; @@ -158,6 +176,24 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } + else if (request == "view_transparent") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewTransparentService, req, cb); + } + else if (request == "view_com") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewCOMService, req, cb); + } + else if (request == "view_inertia") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewInertiaService, req, cb); + } else if (request == "view_wireframes") { ignition::msgs::StringMsg req; diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index e46609bd14..709cfaf857 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -63,6 +63,14 @@ Item { id: secondMenu x: menu.x + menu.width y: menu.y + viewSubmenu.y + MenuItem { + id: viewCOMMenu + text: "Center of Mass" + onTriggered: { + menu.close() + context.OnRequest("view_com", context.entity) + } + } MenuItem { id: viewCollisionsMenu text: "Collisions" @@ -71,6 +79,22 @@ Item { context.OnRequest("view_collisions", context.entity) } } + MenuItem { + id: viewInertiaMenu + text: "Inertia" + onTriggered: { + menu.close() + context.OnRequest("view_inertia", context.entity) + } + } + MenuItem { + id: viewTransparentMenu + text: "Transparent" + onTriggered: { + menu.close() + context.OnRequest("view_transparent", context.entity) + } + } MenuItem { id: viewWireframesMenu text: "Wireframe" @@ -89,6 +113,9 @@ Item { moveToMenu.enabled = false followMenu.enabled = false removeMenu.enabled = false + viewTransparentMenu.enabled = false; + viewCOMMenu.enabled = false; + viewInertiaMenu.enabled = false; viewWireframesMenu.enabled = false; viewCollisionsMenu.enabled = false; @@ -108,6 +135,9 @@ Item { if (context.type == "model" || context.type == "link") { + viewTransparentMenu.enabled = true; + viewCOMMenu.enabled = true; + viewInertiaMenu.enabled = true; viewWireframesMenu.enabled = true; viewCollisionsMenu.enabled = true; } diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc index 6a22b2c096..2935caaf1c 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc @@ -27,19 +27,17 @@ #include #include -#include - #include #include #include #include +#include #include #include #include -#include "ignition/gazebo/components/LogPlaybackStatistics.hh" -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/components/Name.hh" + #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/LogPlaybackStatistics.hh" namespace ignition::gazebo { diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh b/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh index 8a63da97cb..3d2031aa82 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.hh @@ -21,8 +21,8 @@ #include #include -#include #include +#include namespace ignition { diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 7616d18a6e..9320e88be4 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -28,16 +28,16 @@ #include #include -#include #include +#include +#include +#include #include #include #include #include #include #include -#include -#include #include "ignition/gazebo/EntityComponentManager.hh" diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index e8912046b1..9dab3e058e 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +76,7 @@ std::condition_variable g_renderCv; Q_DECLARE_METATYPE(std::string) +Q_DECLARE_METATYPE(ignition::gazebo::RenderSync*) namespace ignition { @@ -95,70 +97,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { bool sendEvent{false}; }; - // - /// \brief Helper class for animating a user camera to move to a target entity - /// todo(anyone) Move this functionality to rendering::Camera class in - /// ign-rendering3 - class MoveToHelper - { - /// \brief Move the camera to look at the specified target - /// param[in] _camera Camera to be moved - /// param[in] _target Target to look at - /// param[in] _duration Duration of the move to animation, in seconds. - /// param[in] _onAnimationComplete Callback function when animation is - /// complete - public: void MoveTo(const rendering::CameraPtr &_camera, - const rendering::NodePtr &_target, double _duration, - std::function _onAnimationComplete); - - /// \brief Move the camera to the specified pose. - /// param[in] _camera Camera to be moved - /// param[in] _target Pose to move to - /// param[in] _duration Duration of the move to animation, in seconds. - /// param[in] _onAnimationComplete Callback function when animation is - /// complete - public: void MoveTo(const rendering::CameraPtr &_camera, - const math::Pose3d &_target, double _duration, - std::function _onAnimationComplete); - - /// \brief Move the camera to look at the specified target - /// param[in] _camera Camera to be moved - /// param[in] _direction The pose to assume relative to the entit(y/ies), - /// (0, 0, 0) indicates to return the camera back to the home pose - /// originally loaded in from the sdf. - /// param[in] _duration Duration of the move to animation, in seconds. - /// param[in] _onAnimationComplete Callback function when animation is - /// complete - public: void LookDirection(const rendering::CameraPtr &_camera, - const math::Vector3d &_direction, const math::Vector3d &_lookAt, - double _duration, std::function _onAnimationComplete); - - /// \brief Add time to the animation. - /// \param[in] _time Time to add in seconds - public: void AddTime(double _time); - - /// \brief Get whether the move to helper is idle, i.e. no animation - /// is being executed. - /// \return True if idle, false otherwise - public: bool Idle() const; - - /// \brief Set the initial camera pose - /// param[in] _pose The init pose of the camera - public: void SetInitCameraPose(const math::Pose3d &_pose); - - /// \brief Pose animation object - public: std::unique_ptr poseAnim; - - /// \brief Pointer to the camera being moved - public: rendering::CameraPtr camera; - - /// \brief Callback function when animation is complete. - public: std::function onAnimationComplete; - - /// \brief Initial pose of the camera used for view angles - public: math::Pose3d initCameraPose; - }; - /// \brief Private data class for IgnRenderer class IgnRendererPrivate { @@ -236,7 +174,16 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: std::string moveToTarget; /// \brief Helper object to move user camera - public: MoveToHelper moveToHelper; + public: ignition::rendering::MoveToHelper moveToHelper; + + /// \brief Target to view as transparent + public: std::string viewTransparentTarget; + + /// \brief Target to view center of mass + public: std::string viewCOMTarget; + + /// \brief Target to view inertia + public: std::string viewInertiaTarget; /// \brief Target to view wireframes public: std::string viewWireframesTarget; @@ -257,12 +204,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Wait for follow target public: bool followTargetWait = false; - /// \brief Offset of camera from taget being followed + /// \brief Offset of camera from target being followed public: math::Vector3d followOffset = math::Vector3d(-5, 0, 3); /// \brief Flag to indicate the follow offset needs to be updated public: bool followOffsetDirty = false; + /// \brief Flag to indicate the follow offset has been updated + public: bool newFollowOffset = true; + /// \brief Follow P gain public: double followPGain = 0.01; @@ -378,6 +328,89 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: math::Vector3d scaleSnap = math::Vector3d::One; }; + /// \brief Qt and Ogre rendering is happening in different threads + /// The original sample 'textureinthread' from Qt used a double-buffer + /// scheme so that the worker (Ogre) thread write to FBO A, while + /// Qt is displaying FBO B. + /// + /// However Qt's implementation doesn't handle all the edge cases + /// (like resizing a window), and also it increases our VRAM + /// consumption in multiple ways (since we have to double other + /// resources as well or re-architect certain parts of the code + /// to avoid it) + /// + /// Thus we just serialize both threads so that when Qt reaches + /// drawing preparation, it halts and Ogre worker thread starts rendering, + /// then resumes when Ogre is done. + /// + /// This code is admitedly more complicated than it should be + /// because Qt's synchronization using signals and slots causes + /// deadlocks when other means of synchronization are introduced. + /// The whole threaded loop should be rewritten. + /// + /// All RenderSync does is conceptually: + /// + /// \code + /// TextureNode::PrepareNode() + /// { + /// renderSync.WaitForWorkerThread(); // Qt thread + /// // WaitForQtThreadAndBlock(); + /// // Now worker thread begins executing what's between + /// // ReleaseQtThreadFromBlock(); + /// continue with qt code... + /// } + /// \endcode + /// + /// + /// For more info see + /// https://github.com/ignitionrobotics/ign-rendering/issues/304 + class RenderSync + { + /// \brief Cond. variable to synchronize rendering on specific events + /// (e.g. texture resize) or for debugging (e.g. keep + /// all API calls sequential) + public: std::mutex mutex; + + /// \brief Cond. variable to synchronize rendering on specific events + /// (e.g. texture resize) or for debugging (e.g. keep + /// all API calls sequential) + public: std::condition_variable cv; + + public: enum class RenderStallState + { + /// Qt is stuck inside WaitForWorkerThread + /// Worker thread can proceed + WorkerCanProceed, + /// Qt is stuck inside WaitForWorkerThread + /// Worker thread is between WaitForQtThreadAndBlock + /// and ReleaseQtThreadFromBlock + WorkerIsProceeding, + /// Worker is stuck inside WaitForQtThreadAndBlock + /// Qt can proceed + QtCanProceed, + /// Do not block + ShuttingDown, + }; + + /// \brief See TextureNode::RenderSync::RenderStallState + public: RenderStallState renderStallState = + RenderStallState::QtCanProceed /*GUARDED_BY(sharedRenderMutex)*/; + + /// \brief Must be called from worker thread when we want to block + /// \param[in] lock Acquired lock. Must be based on this->mutex + public: void WaitForQtThreadAndBlock(std::unique_lock &_lock); + + /// \brief Must be called from worker thread when we are done + /// \param[in] lock Acquired lock. Must be based on this->mutex + public: void ReleaseQtThreadFromBlock(std::unique_lock &_lock); + + /// \brief Must be called from Qt thread periodically + public: void WaitForWorkerThread(); + + /// \brief Must be called from GUI thread when shutting down + public: void Shutdown(); + }; + /// \brief Private data class for RenderWindowItem class RenderWindowItemPrivate { @@ -387,6 +420,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Render thread public : RenderThread *renderThread = nullptr; + /// \brief See RenderSync + public: RenderSync renderSync; + //// \brief Set to true after the renderer is initialized public: bool rendererInit = false; @@ -418,6 +454,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Follow service public: std::string followService; + /// \brief Follow offset service + public: std::string followOffsetService; + /// \brief View angle service public: std::string viewAngleService; @@ -446,6 +485,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// Used when recording in lockstep mode. public: std::mutex renderMutex; + /// \brief View transparent service + public: std::string viewTransparentService; + + /// \brief View center of mass service + public: std::string viewCOMService; + + /// \brief View inertia service + public: std::string viewInertiaService; + /// \brief View wireframes service public: std::string viewWireframesService; @@ -464,11 +512,69 @@ using namespace gazebo; QList RenderWindowItemPrivate::threads; +///////////////////////////////////////////////// +void RenderSync::WaitForQtThreadAndBlock(std::unique_lock &_lock) +{ + this->cv.wait(_lock, [this] + { return this->renderStallState == RenderStallState::WorkerCanProceed || + this->renderStallState == RenderStallState::ShuttingDown; }); + + this->renderStallState = RenderStallState::WorkerIsProceeding; +} + +///////////////////////////////////////////////// +void RenderSync::ReleaseQtThreadFromBlock(std::unique_lock &_lock) +{ + this->renderStallState = RenderStallState::QtCanProceed; + _lock.unlock(); + this->cv.notify_one(); +} + +///////////////////////////////////////////////// +void RenderSync::WaitForWorkerThread() +{ + std::unique_lock lock(this->mutex); + + // Wait until we're clear to go + this->cv.wait( lock, [this] + { + return this->renderStallState == RenderStallState::QtCanProceed || + this->renderStallState == RenderStallState::ShuttingDown; + } ); + + // Worker thread asked us to wait! + this->renderStallState = RenderStallState::WorkerCanProceed; + lock.unlock(); + // Wake up worker thread + this->cv.notify_one(); + lock.lock(); + + // Wait until we're clear to go + this->cv.wait( lock, [this] + { + return this->renderStallState == RenderStallState::QtCanProceed || + this->renderStallState == RenderStallState::ShuttingDown; + } ); +} + +///////////////////////////////////////////////// +void RenderSync::Shutdown() +{ + { + std::unique_lock lock(this->mutex); + + this->renderStallState = RenderStallState::ShuttingDown; + + lock.unlock(); + this->cv.notify_one(); + } +} + ///////////////////////////////////////////////// IgnRenderer::IgnRenderer() : dataPtr(new IgnRendererPrivate) { - this->dataPtr->moveToHelper.initCameraPose = this->cameraPose; + this->dataPtr->moveToHelper.SetInitCameraPose(this->cameraPose); // recorder stats topic std::string recorderStatsTopic = "/gui/record_video/stats"; @@ -489,7 +595,7 @@ RenderUtil *IgnRenderer::RenderUtil() const } ///////////////////////////////////////////////// -void IgnRenderer::Render() +void IgnRenderer::Render(RenderSync *_renderSync) { rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); if (!scene) @@ -503,8 +609,20 @@ void IgnRenderer::Render() IGN_PROFILE_THREAD_NAME("RenderThread"); IGN_PROFILE("IgnRenderer::Render"); + + std::unique_lock lock(_renderSync->mutex); + _renderSync->WaitForQtThreadAndBlock(lock); + if (this->textureDirty) { + // TODO(anyone) If SwapFromThread gets implemented, + // then we only need to lock when texture is dirty + // (but we still need to lock the whole routine if + // debugging from RenderDoc or if user is not willing + // to sacrifice VRAM) + // + // std::unique_lock lock(renderSync->mutex); + // _renderSync->WaitForQtThreadAndBlock(lock); this->dataPtr->camera->SetImageWidth(this->textureSize.width()); this->dataPtr->camera->SetImageHeight(this->textureSize.height()); this->dataPtr->camera->SetAspectRatio(this->textureSize.width() / @@ -518,6 +636,9 @@ void IgnRenderer::Render() this->dataPtr->mouseDirty = true; this->textureDirty = false; + + // TODO(anyone) See SwapFromThread comments + // _renderSync->ReleaseQtThreadFromBlock(lock); } // texture id could change so get the value in every render update @@ -707,7 +828,10 @@ void IgnRenderer::Render() { IGN_PROFILE("IgnRenderer::Render Follow"); if (!this->dataPtr->moveToTarget.empty()) + { + _renderSync->ReleaseQtThreadFromBlock(lock); return; + } rendering::NodePtr followTarget = this->dataPtr->camera->FollowTarget(); if (!this->dataPtr->followTarget.empty()) { @@ -715,7 +839,8 @@ void IgnRenderer::Render() this->dataPtr->followTarget); if (target) { - if (!followTarget || target != followTarget) + if (!followTarget || target != followTarget + || this->dataPtr->newFollowOffset) { this->dataPtr->camera->SetFollowTarget(target, this->dataPtr->followOffset, @@ -725,6 +850,7 @@ void IgnRenderer::Render() this->dataPtr->camera->SetTrackTarget(target); // found target, no need to wait anymore this->dataPtr->followTargetWait = false; + this->dataPtr->newFollowOffset = false; } else if (this->dataPtr->followOffsetDirty) { @@ -831,6 +957,84 @@ void IgnRenderer::Render() } } + // View as transparent + { + IGN_PROFILE("IgnRenderer::Render ViewTransparent"); + if (!this->dataPtr->viewTransparentTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewTransparentTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewTransparent(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewTransparentTarget + << "] to view as transparent" << std::endl; + } + + this->dataPtr->viewTransparentTarget.clear(); + } + } + + // View center of mass + { + IGN_PROFILE("IgnRenderer::Render ViewCOM"); + if (!this->dataPtr->viewCOMTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewCOMTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewCOM(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewCOMTarget + << "] to view center of mass" << std::endl; + } + + this->dataPtr->viewCOMTarget.clear(); + } + } + + // View inertia + { + IGN_PROFILE("IgnRenderer::Render ViewInertia"); + if (!this->dataPtr->viewInertiaTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewInertiaTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewInertia(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewInertiaTarget + << "] to view inertia" << std::endl; + } + + this->dataPtr->viewInertiaTarget.clear(); + } + } + // View wireframes { IGN_PROFILE("IgnRenderer::Render ViewWireframes"); @@ -894,6 +1098,14 @@ void IgnRenderer::Render() // only has an effect in video recording lockstep mode // this notifes ECM to continue updating the scene g_renderCv.notify_one(); + + // TODO(anyone) implement a SwapFromThread for parallel command generation + // See https://github.com/ignitionrobotics/ign-rendering/issues/304 + // if( bForcedSerialization ) + // this->dataPtr->camera->SwapFromThread(); + // else + // _renderSync->ReleaseQtThreadFromBlock(lock); + _renderSync->ReleaseQtThreadFromBlock(lock); } ///////////////////////////////////////////////// @@ -1719,7 +1931,6 @@ void IgnRenderer::HandleMouseViewControl() << std::endl; } - math::Vector3d camWorldPos; if (!this->dataPtr->followTarget.empty()) this->dataPtr->camera->WorldPosition(); @@ -1790,13 +2001,7 @@ void IgnRenderer::HandleMouseViewControl() if (!this->dataPtr->followTarget.empty()) - { - math::Vector3d dPos = this->dataPtr->camera->WorldPosition() - camWorldPos; - if (dPos != math::Vector3d::Zero) - { - this->dataPtr->followOffsetDirty = true; - } - } + this->dataPtr->followOffsetDirty = true; } ///////////////////////////////////////////////// @@ -2060,6 +2265,27 @@ void IgnRenderer::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->moveToPoseValue = _pose; } +///////////////////////////////////////////////// +void IgnRenderer::SetViewTransparentTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewTransparentTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewCOMTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewCOMTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewInertiaTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewInertiaTarget = _target; +} + ///////////////////////////////////////////////// void IgnRenderer::SetViewWireframesTarget(const std::string &_target) { @@ -2118,6 +2344,9 @@ void IgnRenderer::SetFollowOffset(const math::Vector3d &_offset) { std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->followOffset = _offset; + + if (!this->dataPtr->followTarget.empty()) + this->dataPtr->newFollowOffset = true; } ///////////////////////////////////////////////// @@ -2240,10 +2469,11 @@ RenderThread::RenderThread() { RenderWindowItemPrivate::threads << this; qRegisterMetaType(); + qRegisterMetaType("RenderSync*"); } ///////////////////////////////////////////////// -void RenderThread::RenderNext() +void RenderThread::RenderNext(RenderSync *_renderSync) { this->context->makeCurrent(this->surface); @@ -2260,7 +2490,7 @@ void RenderThread::RenderNext() return; } - this->ignRenderer.Render(); + this->ignRenderer.Render(_renderSync); emit TextureReady(this->ignRenderer.textureId, this->ignRenderer.textureSize); } @@ -2279,6 +2509,7 @@ void RenderThread::ShutDown() this->surface->deleteLater(); // Stop event processing, move the thread to GUI and make sure it is deleted. + this->exit(); this->moveToThread(QGuiApplication::instance()->thread()); } @@ -2301,8 +2532,8 @@ void RenderThread::SizeChanged() } ///////////////////////////////////////////////// -TextureNode::TextureNode(QQuickWindow *_window) - : window(_window) +TextureNode::TextureNode(QQuickWindow *_window, RenderSync &_renderSync) + : renderSync(_renderSync), window(_window) { // Our texture node must have a texture, so use the default 0 texture. #if QT_VERSION < QT_VERSION_CHECK(5, 14, 0) @@ -2323,7 +2554,7 @@ TextureNode::~TextureNode() } ///////////////////////////////////////////////// -void TextureNode::NewTexture(int _id, const QSize &_size) +void TextureNode::NewTexture(uint _id, const QSize &_size) { this->mutex.lock(); this->id = _id; @@ -2339,7 +2570,7 @@ void TextureNode::NewTexture(int _id, const QSize &_size) void TextureNode::PrepareNode() { this->mutex.lock(); - int newId = this->id; + uint newId = this->id; QSize sz = this->size; this->id = 0; this->mutex.unlock(); @@ -2371,8 +2602,32 @@ void TextureNode::PrepareNode() // This will notify the rendering thread that the texture is now being // rendered and it can start rendering to the other one. - emit TextureInUse(); + // emit TextureInUse(&this->renderSync); See comment below } + + // NOTE: The original code from Qt samples only emitted when + // newId is not null. + // + // This is correct... for their case. + // However we need to synchronize the threads when resolution changes, + // and we're also currently doing everything in lockstep (i.e. both Qt + // and worker thread are serialized, + // see https://github.com/ignitionrobotics/ign-rendering/issues/304 ) + // + // We need to emit even if newId == 0 because it's safe as long as both + // threads are forcefully serialized and otherwise we may get a + // deadlock (this func. called twice in a row with the worker thread still + // finishing the 1st iteration, may result in a deadlock for newer versions + // of Qt; as WaitForWorkerThread will be called with no corresponding + // WaitForQtThreadAndBlock as the worker thread thinks there are + // no more jobs to do. + // + // If we want these to run in worker thread and stay resolution-synchronized, + // we probably should use a different method of signals and slots + // to send work to the worker thread and get results back + emit TextureInUse(&this->renderSync); + + this->renderSync.WaitForWorkerThread(); } ///////////////////////////////////////////////// @@ -2395,7 +2650,15 @@ RenderWindowItem::RenderWindowItem(QQuickItem *_parent) } ///////////////////////////////////////////////// -RenderWindowItem::~RenderWindowItem() = default; +RenderWindowItem::~RenderWindowItem() +{ + this->dataPtr->renderSync.Shutdown(); + QMetaObject::invokeMethod(this->dataPtr->renderThread, + "ShutDown", + Qt::QueuedConnection); + + this->dataPtr->renderThread->wait(); +} ///////////////////////////////////////////////// void RenderWindowItem::Ready() @@ -2418,10 +2681,6 @@ void RenderWindowItem::Ready() this->dataPtr->renderThread->moveToThread(this->dataPtr->renderThread); - this->connect(this, &QObject::destroyed, - this->dataPtr->renderThread, &RenderThread::ShutDown, - Qt::QueuedConnection); - this->connect(this, &QQuickItem::widthChanged, this->dataPtr->renderThread, &RenderThread::SizeChanged); this->connect(this, &QQuickItem::heightChanged, @@ -2484,7 +2743,7 @@ QSGNode *RenderWindowItem::updatePaintNode(QSGNode *_node, if (!node) { - node = new TextureNode(this->window()); + node = new TextureNode(this->window(), this->dataPtr->renderSync); // Set up connections to get the production of render texture in sync with // vsync on the rendering thread. @@ -2514,7 +2773,8 @@ QSGNode *RenderWindowItem::updatePaintNode(QSGNode *_node, // Get the production of FBO textures started.. QMetaObject::invokeMethod(this->dataPtr->renderThread, "RenderNext", - Qt::QueuedConnection); + Qt::QueuedConnection, + Q_ARG( RenderSync*, &node->renderSync )); } node->setRect(this->boundingRect()); @@ -2788,6 +3048,13 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Follow service on [" << this->dataPtr->followService << "]" << std::endl; + // follow offset + this->dataPtr->followOffsetService = "/gui/follow/offset"; + this->dataPtr->node.Advertise(this->dataPtr->followOffsetService, + &Scene3D::OnFollowOffset, this); + ignmsg << "Follow offset service on [" + << this->dataPtr->followOffsetService << "]" << std::endl; + // view angle this->dataPtr->viewAngleService = "/gui/view_angle"; @@ -2811,6 +3078,27 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Camera pose topic advertised on [" << this->dataPtr->cameraPoseTopic << "]" << std::endl; + // view as transparent service + this->dataPtr->viewTransparentService = "/gui/view/transparent"; + this->dataPtr->node.Advertise(this->dataPtr->viewTransparentService, + &Scene3D::OnViewTransparent, this); + ignmsg << "View as transparent service on [" + << this->dataPtr->viewTransparentService << "]" << std::endl; + + // view center of mass service + this->dataPtr->viewCOMService = "/gui/view/com"; + this->dataPtr->node.Advertise(this->dataPtr->viewCOMService, + &Scene3D::OnViewCOM, this); + ignmsg << "View center of mass service on [" + << this->dataPtr->viewCOMService << "]" << std::endl; + + // view inertia service + this->dataPtr->viewInertiaService = "/gui/view/inertia"; + this->dataPtr->node.Advertise(this->dataPtr->viewInertiaService, + &Scene3D::OnViewInertia, this); + ignmsg << "View inertia service on [" + << this->dataPtr->viewInertiaService << "]" << std::endl; + // view wireframes service this->dataPtr->viewWireframesService = "/gui/view/wireframes"; this->dataPtr->node.Advertise(this->dataPtr->viewWireframesService, @@ -2948,6 +3236,19 @@ bool Scene3D::OnFollow(const msgs::StringMsg &_msg, return true; } +///////////////////////////////////////////////// +bool Scene3D::OnFollowOffset(const msgs::Vector3d &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + math::Vector3d offset = msgs::Convert(_msg); + renderWindow->SetFollowOffset(offset); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, msgs::Boolean &_res) @@ -2987,6 +3288,42 @@ bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) return true; } +///////////////////////////////////////////////// +bool Scene3D::OnViewTransparent(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewTransparentTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewCOM(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewCOMTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewInertia(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewInertiaTarget(_msg.data()); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// bool Scene3D::OnViewWireframes(const msgs::StringMsg &_msg, msgs::Boolean &_res) @@ -3281,6 +3618,24 @@ void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->renderThread->ignRenderer.SetMoveToPose(_pose); } +///////////////////////////////////////////////// +void RenderWindowItem::SetViewTransparentTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewTransparentTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewCOMTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewCOMTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewInertiaTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewInertiaTarget(_target); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetViewWireframesTarget(const std::string &_target) { @@ -3483,158 +3838,6 @@ void RenderWindowItem::HandleKeyRelease(QKeyEvent *_e) // } // -//////////////////////////////////////////////// -void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera, - const ignition::math::Pose3d &_target, - double _duration, std::function _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "move_to", _duration, false); - this->onAnimationComplete = std::move(_onAnimationComplete); - - math::Pose3d start = _camera->WorldPose(); - - common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0); - key->Translation(start.Pos()); - key->Rotation(start.Rot()); - - key = this->poseAnim->CreateKeyFrame(_duration); - if (_target.Pos().IsFinite()) - key->Translation(_target.Pos()); - else - key->Translation(start.Pos()); - - if (_target.Rot().IsFinite()) - key->Rotation(_target.Rot()); - else - key->Rotation(start.Rot()); -} - -//////////////////////////////////////////////// -void MoveToHelper::MoveTo(const rendering::CameraPtr &_camera, - const rendering::NodePtr &_target, - double _duration, std::function _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "move_to", _duration, false); - this->onAnimationComplete = std::move(_onAnimationComplete); - - math::Pose3d start = _camera->WorldPose(); - - // todo(anyone) implement bounding box function in rendering to get - // target size and center. - // Assume fixed size and target world position is its center - math::Box targetBBox(1.0, 1.0, 1.0); - math::Vector3d targetCenter = _target->WorldPosition(); - math::Vector3d dir = targetCenter - start.Pos(); - dir.Correct(); - dir.Normalize(); - - // distance to move - double maxSize = targetBBox.Size().Max(); - double dist = start.Pos().Distance(targetCenter) - maxSize; - - // Scale to fit in view - double hfov = this->camera->HFOV().Radian(); - double offset = maxSize*0.5 / std::tan(hfov/2.0); - - // End position and rotation - math::Vector3d endPos = start.Pos() + dir*(dist - offset); - math::Quaterniond endRot = - math::Matrix4d::LookAt(endPos, targetCenter).Rotation(); - math::Pose3d end(endPos, endRot); - - common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0); - key->Translation(start.Pos()); - key->Rotation(start.Rot()); - - key = this->poseAnim->CreateKeyFrame(_duration); - key->Translation(end.Pos()); - key->Rotation(end.Rot()); -} - -//////////////////////////////////////////////// -void MoveToHelper::LookDirection(const rendering::CameraPtr &_camera, - const math::Vector3d &_direction, const math::Vector3d &_lookAt, - double _duration, std::function _onAnimationComplete) -{ - this->camera = _camera; - this->poseAnim = std::make_unique( - "view_angle", _duration, false); - this->onAnimationComplete = std::move(_onAnimationComplete); - - math::Pose3d start = _camera->WorldPose(); - - // Look at world origin unless there are visuals selected - // Keep current distance to look at target - math::Vector3d camPos = _camera->WorldPose().Pos(); - double distance = std::fabs((camPos - _lookAt).Length()); - - // Calculate camera position - math::Vector3d endPos = _lookAt - _direction * distance; - - // Calculate camera orientation - math::Quaterniond endRot = - ignition::math::Matrix4d::LookAt(endPos, _lookAt).Rotation(); - - // Move camera to that pose - common::PoseKeyFrame *key = this->poseAnim->CreateKeyFrame(0); - key->Translation(start.Pos()); - key->Rotation(start.Rot()); - - // Move camera back to initial pose - if (_direction == math::Vector3d::Zero) - { - endPos = this->initCameraPose.Pos(); - endRot = this->initCameraPose.Rot(); - } - - key = this->poseAnim->CreateKeyFrame(_duration); - key->Translation(endPos); - key->Rotation(endRot); -} - -//////////////////////////////////////////////// -void MoveToHelper::AddTime(double _time) -{ - if (!this->camera || !this->poseAnim) - return; - - common::PoseKeyFrame kf(0); - - this->poseAnim->AddTime(_time); - this->poseAnim->InterpolatedKeyFrame(kf); - - math::Pose3d offset(kf.Translation(), kf.Rotation()); - - this->camera->SetWorldPose(offset); - - if (this->poseAnim->Length() <= this->poseAnim->Time()) - { - if (this->onAnimationComplete) - { - this->onAnimationComplete(); - } - this->camera.reset(); - this->poseAnim.reset(); - this->onAnimationComplete = nullptr; - } -} - -//////////////////////////////////////////////// -bool MoveToHelper::Idle() const -{ - return this->poseAnim == nullptr; -} - -//////////////////////////////////////////////// -void MoveToHelper::SetInitCameraPose(const math::Pose3d &_pose) -{ - this->initCameraPose = _pose; -} - // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::Scene3D, ignition::gui::Plugin) diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 44b15f4510..caf21a8102 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -140,6 +140,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnFollow(const msgs::StringMsg &_msg, msgs::Boolean &_res); + /// \brief Callback for a follow offset request + /// \param[in] _msg Request message to set the camera's follow offset. + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnFollowOffset(const msgs::Vector3d &_msg, + msgs::Boolean &_res); + /// \brief Callback for a view angle request /// \param[in] _msg Request message to set the camera to. /// \param[in] _res Response data @@ -154,6 +161,29 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res); + /// \brief Callback for view as transparent request + /// \param[in] _msg Request message to set the target to view as + /// transparent + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewTransparent(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view center of mass request + /// \param[in] _msg Request message to set the target to view center of + /// mass + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewCOM(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view inertia request + /// \param[in] _msg Request message to set the target to view inertia + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewInertia(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + /// \brief Callback for view wireframes request /// \param[in] _msg Request message to set the target to view wireframes /// \param[in] _res Response data @@ -180,6 +210,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: std::unique_ptr dataPtr; }; + class RenderSync; + /// \brief Ign-rendering renderer. /// All ign-rendering calls should be performed inside this class as it makes /// sure that opengl calls in the underlying render engine do not interfere @@ -197,7 +229,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: ~IgnRenderer() override; /// \brief Main render function - public: void Render(); + /// \param[in] _renderSync RenderSync to safely + /// synchronize Qt and worker thread (this) + public: void Render(RenderSync *_renderSync); /// \brief Initialize the render engine public: void Initialize(); @@ -266,6 +300,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The world pose to set the camera to. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \brief View the specified target as transparent + /// \param[in] _target Target to view as transparent + public: void SetViewTransparentTarget(const std::string &_target); + + /// \brief View center of mass of the specified target + /// \param[in] _target Target to view center of mass + public: void SetViewCOMTarget(const std::string &_target); + + /// \brief View inertia of the specified target + /// \param[in] _target Target to view inertia + public: void SetViewInertiaTarget(const std::string &_target); + /// \brief View wireframes of the specified target /// \param[in] _target Target to view wireframes public: void SetViewWireframesTarget(const std::string &_target); @@ -472,7 +518,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { bool _waitForTarget); /// \brief Render texture id - public: GLuint textureId = 0u; + /// Values is constantly constantly cycled/swapped/changed + /// from a worker thread + /// Don't read this directly + public: GLuint textureId; /// \brief Initial Camera pose public: math::Pose3d cameraPose = math::Pose3d(0, 0, 2, 0, 0.4, 0); @@ -506,7 +555,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: RenderThread(); /// \brief Render the next frame - public slots: void RenderNext(); + /// \param[in] _renderSync RenderSync to safely + /// synchronize Qt and worker thread (this) + public slots: void RenderNext(RenderSync *renderSync); /// \brief Shutdown the thread and the render engine public slots: void ShutDown(); @@ -518,7 +569,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// to be displayed /// \param[in] _id GLuid of the opengl texture /// \param[in] _size Size of the texture - signals: void TextureReady(int _id, const QSize &_size); + signals: void TextureReady(uint _id, const QSize &_size); /// \brief Offscreen surface to render to public: QOffscreenSurface *surface = nullptr; @@ -620,6 +671,18 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The new camera pose in the world frame. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \brief View the specified target as transparent + /// \param[in] _target Target to view as transparent + public: void SetViewTransparentTarget(const std::string &_target); + + /// \brief View center of mass of the specified target + /// \param[in] _target Target to view center of mass + public: void SetViewCOMTarget(const std::string &_target); + + /// \brief View inertia of the specified target + /// \param[in] _target Target to view inertia + public: void SetViewInertiaTarget(const std::string &_target); + /// \brief View wireframes of the specified target /// \param[in] _target Target to view wireframes public: void SetViewWireframesTarget(const std::string &_target); @@ -751,7 +814,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Constructor /// \param[in] _window Parent window - public: explicit TextureNode(QQuickWindow *_window); + /// \param[in] _renderSync RenderSync to safely + /// synchronize Qt (this) and worker thread + public: explicit TextureNode(QQuickWindow *_window, + RenderSync &_renderSync); /// \brief Destructor public: ~TextureNode() override; @@ -760,7 +826,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// store the texture id and size and schedule an update on the window. /// \param[in] _id OpenGL render texture Id /// \param[in] _size Texture size - public slots: void NewTexture(int _id, const QSize &_size); + public slots: void NewTexture(uint _id, const QSize &_size); /// \brief Before the scene graph starts to render, we update to the /// pending texture @@ -768,14 +834,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Signal emitted when the texture is being rendered and renderer /// can start rendering next frame - signals: void TextureInUse(); + /// \param[in] _renderSync RenderSync to send to the worker thread + signals: void TextureInUse(RenderSync *_renderSync); /// \brief Signal emitted when a new texture is ready to trigger window /// update signals: void PendingNewTexture(); /// \brief OpenGL texture id - public: int id = 0; + public: uint id = 0; /// \brief Texture size public: QSize size = QSize(0, 0); @@ -783,6 +850,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Mutex to protect the texture variables public: QMutex mutex; + /// \brief See RenderSync + public: RenderSync &renderSync; + /// \brief Qt's scene graph texture public: QSGTexture *texture = nullptr; diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index e4d524d997..f4e71ad0b2 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -32,8 +32,6 @@ #include #include -#include "ignition/gazebo/EntityComponentManager.hh" - namespace ignition::gazebo { class ShapesPrivate diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 656743721c..c273ec43ea 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -38,9 +38,6 @@ #include #include -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo @@ -165,42 +162,7 @@ void TransformControl::SnapToGrid() ///////////////////////////////////////////////// void TransformControl::LoadGrid() { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - igndbg << "More than one engine is available. " - << "Grid config plugin will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Grid plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto scene = engine->SceneByIndex(0); - if (!scene) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized() || scene->VisualCount() == 0) - { - return; - } + auto scene = rendering::sceneFromFirstRenderEngine(); // load grid // if gridPtr found, load the existing gridPtr to class diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index 3e9df0964c..84406d718f 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -30,10 +30,6 @@ #include #include -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/EntityComponentManager.hh" - namespace ignition::gazebo { class VideoRecorderPrivate diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index d8f6eb4f5e..7c64762be2 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -18,6 +18,7 @@ #include "ViewAngle.hh" #include +#include #include #include @@ -42,6 +43,12 @@ namespace ignition::gazebo /// \brief View Control service name public: std::string viewControlService; + + /// \brief Move gui camera to pose service name + public: std::string moveToPoseService; + + /// \brief gui camera pose + public: math::Pose3d camPose; }; } @@ -68,6 +75,14 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) // view control requests this->dataPtr->viewControlService = "/gui/camera/view_control"; + + // Subscribe to camera pose + std::string topic = "/gui/camera/pose"; + this->dataPtr->node.Subscribe( + topic, &ViewAngle::CamPoseCb, this); + + // Move to pose service + this->dataPtr->moveToPoseService = "/gui/move_to/pose"; } ///////////////////////////////////////////////// @@ -113,6 +128,51 @@ void ViewAngle::OnViewControl(const QString &_controller) this->dataPtr->node.Request(this->dataPtr->viewControlService, req, cb); } +///////////////////////////////////////////////// +QList ViewAngle::CamPose() const +{ + return QList({ + this->dataPtr->camPose.Pos().X(), + this->dataPtr->camPose.Pos().Y(), + this->dataPtr->camPose.Pos().Z(), + this->dataPtr->camPose.Rot().Roll(), + this->dataPtr->camPose.Rot().Pitch(), + this->dataPtr->camPose.Rot().Yaw() + }); +} + +///////////////////////////////////////////////// +void ViewAngle::SetCamPose(double _x, double _y, double _z, + double _roll, double _pitch, double _yaw) +{ + this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw); + + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending move camera to pose request" << std::endl; + }; + + ignition::msgs::GUICamera req; + msgs::Set(req.mutable_pose(), this->dataPtr->camPose); + + this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); +} + +///////////////////////////////////////////////// +void ViewAngle::CamPoseCb(const msgs::Pose &_msg) +{ + std::lock_guard lock(this->dataPtr->mutex); + math::Pose3d pose = msgs::Convert(_msg); + + if (pose != this->dataPtr->camPose) + { + this->dataPtr->camPose = pose; + this->CamPoseChanged(); + } +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ViewAngle, ignition::gui::Plugin) diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index 9dbf39e818..e655ea0371 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -18,6 +18,8 @@ #ifndef IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ #define IGNITION_GAZEBO_GUI_VIEWANGLE_HH_ +#include + #include #include @@ -36,6 +38,13 @@ namespace gazebo { Q_OBJECT + /// \brief gui camera pose (QList order is x, y, z, roll, pitch, yaw) + Q_PROPERTY( + QList camPose + READ CamPose + NOTIFY CamPoseChanged + ) + /// \brief Constructor public: ViewAngle(); @@ -58,6 +67,22 @@ namespace gazebo /// \param[in] _mode New camera view controller public slots: void OnViewControl(const QString &_controller); + /// \brief Get the current gui camera pose. + public: Q_INVOKABLE QList CamPose() const; + + /// \brief Notify that the gui camera pose has changed. + signals: void CamPoseChanged(); + + /// \brief Callback to update gui camera pose + /// \param[in] _x, _y, _z cartesion coordinates + /// \param[in] _roll, _pitch, _yaw principal coordinates + public slots: void SetCamPose(double _x, double _y, double _z, + double _roll, double _pitch, double _yaw); + + /// \brief Callback for retrieving gui camera pose + /// \param[in] _msg Pose message + public: void CamPoseCb(const msgs::Pose &_msg); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 43c3542e80..54a2de4dbc 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -20,21 +20,26 @@ import QtQuick.Controls.Material 2.2 import QtQuick.Controls.Material.impl 2.2 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 +import "qrc:/qml" -ToolBar { - Layout.minimumWidth: 200 - Layout.minimumHeight: 200 +ColumnLayout { + Layout.minimumWidth: 320 + Layout.minimumHeight: 380 + anchors.fill: parent - background: Rectangle { - color: "transparent" - } + ToolBar { + Layout.fillWidth: true + background: Rectangle { + color: "transparent" + } - ButtonGroup { - id: group - } + ButtonGroup { + id: group + } - ColumnLayout { GridLayout { + id: views + anchors.horizontalCenter: parent.horizontalCenter columns: 4 ToolButton { id: top @@ -184,21 +189,156 @@ ToolBar { } } } - RowLayout { - ComboBox { - currentIndex: 0 - model: ListModel { - id: controller - ListElement {text: "Orbit View Control"} - ListElement {text: "Orthographic View Control"} - } - Layout.fillWidth: true - Layout.minimumWidth: 280 - Layout.margins: 10 - onCurrentIndexChanged: { - ViewAngle.OnViewControl(controller.get(currentIndex).text) - } - } + } + + // Projection + ComboBox { + currentIndex: 0 + model: ListModel { + id: controller + ListElement {text: "Orbit View Control"} + ListElement {text: "Orthographic View Control"} + } + Layout.fillWidth: true + Layout.minimumWidth: 280 + Layout.margins: 10 + onCurrentIndexChanged: { + ViewAngle.OnViewControl(controller.get(currentIndex).text) + } + } + + // Set camera pose + Text { + text: "Camera Pose" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + } + + GridLayout { + width: parent.width + columns: 6 + + Text { + text: "X (m)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: x + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: ViewAngle.camPose[0] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Y (m)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: y + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 1 + value: ViewAngle.camPose[1] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 2 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 1 + value: ViewAngle.camPose[2] + maximumValue: 1000000 + minimumValue: -1000000 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + + Text { + text: "Roll (rad)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 2 + leftPadding: 5 } + IgnSpinBox { + id: roll + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 3 + value: ViewAngle.camPose[3] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Pitch (rad)" + color: "dimgrey" + Layout.row: 1 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: pitch + Layout.fillWidth: true + Layout.row: 1 + Layout.column: 3 + value: ViewAngle.camPose[4] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Yaw (rad)" + color: "dimgrey" + Layout.row: 2 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: yaw + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 3 + value: ViewAngle.camPose[5] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + } + + // Bottom spacer + Item { + width: 10 + Layout.fillHeight: true } } diff --git a/src/gui/plugins/visualize_contacts/CMakeLists.txt b/src/gui/plugins/visualize_contacts/CMakeLists.txt new file mode 100644 index 0000000000..3e377f6599 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(VisualizeContacts + SOURCES + VisualizeContacts.cc + QT_HEADERS + VisualizeContacts.hh + PUBLIC_LINK_LIBS + ${IGNITION-RENDERING_LIBRARIES} +) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc new file mode 100644 index 0000000000..4420d90d05 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include + +#include +#include + +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +#include "VisualizeContacts.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + /// \brief Private data class for VisualizeContacts + class VisualizeContactsPrivate + { + /// \brief Creates ContactSensorData for Collision components without a + /// Contact Sensor by requesting the /enable_contact service + /// \param[in] Reference to the GUI Entity Component Manager + public: void CreateCollisionData(EntityComponentManager &_ecm); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Current state of the checkbox + public: bool checkboxState{false}; + + /// \brief Previous state of the checkbox + public: bool checkboxPrevState{false}; + + /// \brief Message for visualizing contact positions + public: ignition::msgs::Marker positionMarkerMsg; + + /// \brief Radius of the visualized contact sphere + public: double contactRadius{0.10}; + + /// \brief Update time of the markers in milliseconds + public: int64_t markerLifetime{200}; + + /// \brief Simulation time for the last markers update + public: std::chrono::steady_clock::duration lastMarkersUpdateTime{0}; + + /// \brief Mutex for variable mutated by the checkbox and spinboxes + /// callbacks. + /// The variables are: checkboxState, contactRadius and markerLifetime + public: std::mutex serviceMutex; + + /// \brief Initialization flag + public: bool initialized{false}; + + /// \brief Name of the world + public: std::string worldName; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +VisualizeContacts::VisualizeContacts() + : GuiSystem(), dataPtr(new VisualizeContactsPrivate) +{ +} + +///////////////////////////////////////////////// +VisualizeContacts::~VisualizeContacts() = default; + +///////////////////////////////////////////////// +void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Visualize contacts"; + + // Configure Marker messages for position of the contacts + + // Blue spheres for positions + + // Create the marker message + this->dataPtr->positionMarkerMsg.set_ns("positions"); + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->positionMarkerMsg.set_type( + ignition::msgs::Marker::SPHERE); + this->dataPtr->positionMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()-> + set_sec(0); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->markerLifetime * 1000000); + + // Set material properties + ignition::msgs::Set( + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + // Set contact position scale + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); +} + +///////////////////////////////////////////////// +void VisualizeContacts::OnVisualize(bool _checked) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->checkboxState = _checked; +} + +////////////////////////////////////////////////// +void VisualizeContacts::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("VisualizeContacts::Update"); + + if (!this->dataPtr->initialized) + { + // Get the name of the world + if (this->dataPtr->worldName.empty()) + { + _ecm.Each( + [&](const Entity &, + const components::World *, + const components::Name *_name) -> bool + { + // We assume there's only one world + this->dataPtr->worldName = _name->Data(); + return false; + }); + } + + // Enable collisions + this->dataPtr->CreateCollisionData(_ecm); + this->dataPtr->initialized = true; + } + + { + std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->checkboxPrevState && !this->dataPtr->checkboxState) + { + // Remove the markers + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::DELETE_ALL); + + igndbg << "Removing markers..." << std::endl; + this->dataPtr->node.Request( + "/marker", this->dataPtr->positionMarkerMsg); + + // Change action in case checkbox is checked again + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + } + + this->dataPtr->checkboxPrevState = this->dataPtr->checkboxState; + if (!this->dataPtr->checkboxState) + return; + } + + // Only publish markers if enough time has passed + auto timeDiff = + std::chrono::duration_cast(_info.simTime - + this->dataPtr->lastMarkersUpdateTime); + + if (timeDiff.count() < this->dataPtr->markerLifetime) + return; + + // Store simulation time + this->dataPtr->lastMarkersUpdateTime = _info.simTime; + + // Get the contacts and publish them + // Since we are setting a lifetime for the markers, we get all the + // contacts instead of getting new and removed ones + + // Variable for setting the markers id through the iteration + int markerID = 1; + _ecm.Each( + [&](const Entity &, + const components::ContactSensorData *_contacts) -> bool + { + for (const auto &contact : _contacts->Data().contact()) + { + for (int i = 0; i < contact.position_size(); ++i) + { + // Set marker id, poses and request service + this->dataPtr->positionMarkerMsg.set_id(markerID++); + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_pose(), + ignition::math::Pose3d(contact.position(i).x(), + contact.position(i).y(), contact.position(i).z(), + 0, 0, 0)); + + this->dataPtr->node.Request( + "/marker", this->dataPtr->positionMarkerMsg); + } + } + return true; + }); +} + +////////////////////////////////////////////////// +void VisualizeContactsPrivate::CreateCollisionData( + EntityComponentManager &_ecm) +{ + // Collisions can't be enabled with _ecm given that this is a GUI plugin and + // it doesn't run in the same process as the physics. + // We use the world//enable_collision service instead. + _ecm.Each( + [&](const Entity &_entity, + const components::Collision *) -> bool + { + // Check if ContactSensorData has already been created + bool collisionHasContactSensor = + _ecm.EntityHasComponentType(_entity, + components::ContactSensorData::typeId); + + if (collisionHasContactSensor) + { + igndbg << "ContactSensorData detected in collision [" << _entity << "]" + << std::endl; + return true; + } + + // Request service for enabling collision + msgs::Entity req; + req.set_id(_entity); + req.set_type(msgs::Entity::COLLISION); + + msgs::Boolean res; + bool result; + unsigned int timeout = 50; + std::string service = "/world/" + this->worldName + "/enable_collision"; + + this->node.Request(service, req, timeout, res, result); + + return true; + }); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdateRadius(double _radius) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->contactRadius = _radius; + + // Set scale + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdatePeriod(double _period) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->markerLifetime = _period; + + // Set markers lifetime + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()->set_nsec(_period * 1000000); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::VisualizeContacts, + ignition::gui::Plugin) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh new file mode 100644 index 0000000000..f1c6211084 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_VISUALIZECONTACTS_HH_ +#define IGNITION_GAZEBO_GUI_VISUALIZECONTACTS_HH_ + +#include + +#include + +#include "ignition/gui/qt.h" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + class VisualizeContactsPrivate; + + /// \brief Visualize the contacts returned by the Physics plugin. Use the + /// checkbox to turn visualization on or off and spin boxes to change + /// the size of the markers. + class VisualizeContacts : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: VisualizeContacts(); + + /// \brief Destructor + public: ~VisualizeContacts() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback when checkbox state is changed + /// \param[in] _checked indicates show or hide contacts + public slots: void OnVisualize(bool _checked); + + /// \brief Update the radius of the contact + /// \param[in] _radius new radius of the contact + public slots: void UpdateRadius(double _radius); + + /// \brief Update the update period of the markers + /// \param[in] _period new update period + public slots: void UpdatePeriod(double _period); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml new file mode 100644 index 0000000000..0562257e92 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 6 + columnSpacing: 10 + Layout.minimumWidth: 250 + Layout.minimumHeight: 400 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Left spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + CheckBox { + Layout.alignment: Qt.AlignHCenter + id: visualizeContacts + Layout.columnSpan: 4 + text: qsTr("Show Contacts") + checked: false + onClicked: { + VisualizeContacts.OnVisualize(checked) + } + } + + // Right spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + Text { + Layout.columnSpan: 2 + id: radiusText + color: "dimgrey" + text: "Radius (m)" + } + + IgnSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: radius + maximumValue: 2.00 + minimumValue: 0.01 + value: 0.10 + decimals: 2 + stepSize: 0.05 + onEditingFinished: VisualizeContacts.UpdateRadius(radius.value) + } + + Text { + Layout.columnSpan: 2 + id: updatePeriodText + color: "dimgrey" + text: "Update period (ms)" + } + + IgnSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: updatePeriod + maximumValue: 2000 + minimumValue: 10 + value: 200 + decimals: 0 + stepSize: 50 + onEditingFinished: VisualizeContacts.UpdatePeriod(updatePeriod.value) + } + + // Bottom spacer + Item { + Layout.columnSpan: 4 + Layout.fillHeight: true + } +} diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc b/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc new file mode 100644 index 0000000000..fe1bf74e0a --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc @@ -0,0 +1,5 @@ + + + VisualizeContacts.qml + + diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 837540ebb8..51bf931ec9 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -57,6 +57,7 @@ #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" @@ -301,17 +302,64 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _node Node to be restored. public: void LowlightNode(const rendering::NodePtr &_node); + /// \brief New center of mass visuals to be created + public: std::vector newCOMVisuals; + + /// \brief A list of links used to create new center of mass visuals + public: std::vector newCOMLinks; + + /// \brief A map of link entities and if their center of mass visuals + /// are currently visible + public: std::map viewingCOM; + + /// \brief New inertias to be created + public: std::vector newInertias; + + /// \brief A map of links and their center of mass visuals + public: std::map linkToCOMVisuals; + + /// \brief Finds the child links for given entity from the ECM + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to find child links + /// \return A vector of child links found for the entity + public: std::vector FindChildLinksFromECM( + const EntityComponentManager &_ecm, const Entity &_entity); + + /// \brief Finds the links (inertial parent) that are used to create child + /// inertia and center of mass visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindInertialLinks(const EntityComponentManager &_ecm); + + /// \brief A list of links used to create new inertia visuals + public: std::vector newInertiaLinks; + + /// \brief A map of entity ids and their inertials + public: std::map entityInertials; + + /// \brief A map of link entities and if their inertias are currently + /// visible + public: std::map viewingInertias; + + /// \brief A map of links and their inertia visuals + public: std::map linkToInertiaVisuals; + /// \brief New wireframe visuals to be toggled public: std::vector newWireframes; + /// \brief New wireframe visuals to be toggled + public: std::vector newTransparentEntities; + /// \brief Finds the links (visual parent) that are used to toggle wireframe - /// views for visuals in RenderUtil::Update + /// and transparent view for visuals in RenderUtil::Update /// \param[in] _ecm The entity-component manager - public: void FindWireframeVisualLinks(const EntityComponentManager &_ecm); + public: void PopulateViewModeVisualLinks(const EntityComponentManager &_ecm); - /// \brief A list of links used to toggle different views for visuals + /// \brief A list of links used to toggle wireframe mode for visuals public: std::vector newWireframeVisualLinks; + /// \brief A list of links used to toggle transparent mode for visuals + public: std::vector newTransparentVisualLinks; + /// \brief A map of link entities and their corresponding children visuals public: std::map> linkToVisualEntities; @@ -319,6 +367,10 @@ class ignition::gazebo::RenderUtilPrivate /// visible public: std::map viewingWireframes; + /// \brief A map of created transparent visuals and if they are currently + /// visible + public: std::map viewingTransparent; + /// \brief New collisions to be created public: std::vector newCollisions; @@ -349,8 +401,44 @@ class ignition::gazebo::RenderUtilPrivate /// \brief A map of entity id to thermal camera sensor configuration /// properties. The elements in the tuple are: /// - public:std::unordered_map> thermalCameraData; + + /// \brief A helper function that removes the sensor associated with an + /// entity, if an associated sensor exists. This should be called in + /// RenderUtil::Update. + /// \param[in] _entity The entity that should be checked for an associated + /// sensor. + public: void RemoveSensor(const Entity _entity); + + /// \brief A helper function that removes the bounding box associated with an + /// entity, if an associated bounding box exists. This should be called in + /// RenderUtil::Update. + /// \param[in] _entity The entity that should be checked for an associated + /// bounding box. + public: void RemoveBoundingBox(const Entity _entity); + + /// \brief A helper function for updating lights. This should be called in + /// RenderUtil::Update. + /// \param[in] _entityLights A map of entity IDs to their light updates. + public: void UpdateLights( + const std::unordered_map &_entityLights); + + /// \brief A helper function for updating the thermal camera. This should be + /// called in RenderUtil::Update. + /// \param[in] _thermalCamData The thermal camera data that needs to be + /// updated. + /// \sa thermalCameraData + public: void UpdateThermalCamera(const std::unordered_map> &_thermalCamData); + + /// \brief Helper function for updating animation. This should be called in + /// RenderUtil::Update. + /// \param[in] _actorAnimationData A map of entities to their animation update + /// data. + /// \sa actorManualSkeletonUpdate + public: void UpdateAnimation(const std::unordered_map &_actorAnimationData); }; ////////////////////////////////////////////////// @@ -486,51 +574,117 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); - this->dataPtr->FindWireframeVisualLinks(_ecm); + this->dataPtr->PopulateViewModeVisualLinks(_ecm); + this->dataPtr->FindInertialLinks(_ecm); this->dataPtr->FindCollisionLinks(_ecm); } ////////////////////////////////////////////////// -void RenderUtilPrivate::FindWireframeVisualLinks( - const EntityComponentManager &_ecm) +std::vector RenderUtilPrivate::FindChildLinksFromECM( + const EntityComponentManager &_ecm, const Entity &_entity) { - if (this->newWireframes.empty()) - return; + std::vector links; + if (_ecm.EntityMatches(_entity, + std::set{components::Model::typeId})) + { + std::stack modelStack; + modelStack.push(_entity); - for (const auto &entity : this->newWireframes) + std::vector childLinks, childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Link()); + links.insert(links.end(), + childLinks.begin(), + childLinks.end()); + + childModels = + _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Model()); + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + else if (_ecm.EntityMatches(_entity, + std::set{components::Link::typeId})) + { + links.push_back(_entity); + } + return links; +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::FindInertialLinks(const EntityComponentManager &_ecm) +{ + for (const auto &entity : this->newInertias) { std::vector links; if (_ecm.EntityMatches(entity, - std::set{components::Model::typeId})) + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else { - std::stack modelStack; - modelStack.push(entity); + ignerr << "Entity [" << entity + << "] for viewing inertia must be a model or link" + << std::endl; + continue; + } - std::vector childLinks, childModels; - while (!modelStack.empty()) - { - Entity model = modelStack.top(); - modelStack.pop(); - - childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), - components::Link()); - links.insert(links.end(), - childLinks.begin(), - childLinks.end()); - - childModels = - _ecm.EntitiesByComponents(components::ParentEntity(model), - components::Model()); - for (const auto &childModel : childModels) - { - modelStack.push(childModel); - } - } + this->newInertiaLinks.insert(this->newInertiaLinks.end(), + links.begin(), + links.end()); + } + this->newInertias.clear(); + + for (const auto &entity : this->newCOMVisuals) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing center of mass must be a model or link" + << std::endl; + continue; } - else if (_ecm.EntityMatches(entity, + + this->newCOMLinks.insert(this->newCOMLinks.end(), + links.begin(), + links.end()); + } + this->newCOMVisuals.clear(); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::PopulateViewModeVisualLinks( + const EntityComponentManager &_ecm) +{ + // Find links to toggle wireframes + for (const auto &entity : this->newWireframes) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, std::set{components::Link::typeId})) { - links.push_back(entity); + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); } else { @@ -545,6 +699,32 @@ void RenderUtilPrivate::FindWireframeVisualLinks( links.end()); } this->newWireframes.clear(); + + // Find links to view as transparent + for (const auto &entity : this->newTransparentEntities) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing as transparent must be a model or link" + << std::endl; + continue; + } + + this->newTransparentVisualLinks.insert( + this->newTransparentVisualLinks.end(), + links.begin(), + links.end()); + } + this->newTransparentEntities.clear(); } ////////////////////////////////////////////////// @@ -557,36 +737,11 @@ void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) { std::vector links; if (_ecm.EntityMatches(entity, - std::set{components::Model::typeId})) - { - std::stack modelStack; - modelStack.push(entity); - - std::vector childLinks, childModels; - while (!modelStack.empty()) - { - Entity model = modelStack.top(); - modelStack.pop(); - - childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), - components::Link()); - links.insert(links.end(), - childLinks.begin(), - childLinks.end()); - - childModels = - _ecm.EntitiesByComponents(components::ParentEntity(model), - components::Model()); - for (const auto &childModel : childModels) - { - modelStack.push(childModel); - } - } - } - else if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, std::set{components::Link::typeId})) { - links.push_back(entity); + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); } else { @@ -645,6 +800,10 @@ void RenderUtil::Update() auto actorTransforms = std::move(this->dataPtr->actorTransforms); auto actorAnimationData = std::move(this->dataPtr->actorAnimationData); auto entityTemp = std::move(this->dataPtr->entityTemp); + auto newTransparentVisualLinks = + std::move(this->dataPtr->newTransparentVisualLinks); + auto newInertiaLinks = std::move(this->dataPtr->newInertiaLinks); + auto newCOMLinks = std::move(this->dataPtr->newCOMLinks); auto newWireframeVisualLinks = std::move(this->dataPtr->newWireframeVisualLinks); auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); @@ -665,6 +824,9 @@ void RenderUtil::Update() this->dataPtr->actorTransforms.clear(); this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->newTransparentVisualLinks.clear(); + this->dataPtr->newInertiaLinks.clear(); + this->dataPtr->newCOMLinks.clear(); this->dataPtr->newWireframeVisualLinks.clear(); this->dataPtr->newCollisionLinks.clear(); this->dataPtr->thermalCameraData.clear(); @@ -708,21 +870,8 @@ void RenderUtil::Update() this->dataPtr->selectedEntities.end()); this->dataPtr->sceneManager.RemoveEntity(entity.first); - // delete associated sensor, if existing - auto sensorEntityIt = this->dataPtr->sensorEntities.find(entity.first); - if (sensorEntityIt != this->dataPtr->sensorEntities.end()) - { - this->dataPtr->removeSensorCb(entity.first); - this->dataPtr->sensorEntities.erase(sensorEntityIt); - } - - // delete associated bounding box, if existing - auto wireBoxIt = this->dataPtr->wireBoxes.find(entity.first); - if (wireBoxIt != this->dataPtr->wireBoxes.end()) - { - this->dataPtr->scene->DestroyVisual(wireBoxIt->second->Parent()); - this->dataPtr->wireBoxes.erase(wireBoxIt); - } + this->dataPtr->RemoveSensor(entity.first); + this->dataPtr->RemoveBoundingBox(entity.first); } } @@ -834,101 +983,8 @@ void RenderUtil::Update() } } - // update lights - { - IGN_PROFILE("RenderUtil::Update Lights"); - for (const auto &light : entityLights) { - auto node = this->dataPtr->sceneManager.NodeById(light.first); - if (!node) - continue; - auto l = std::dynamic_pointer_cast(node); - if (l) - { - if (light.second.has_diffuse()) - { - if (l->DiffuseColor() != msgs::Convert(light.second.diffuse())) - l->SetDiffuseColor(msgs::Convert(light.second.diffuse())); - } - if (light.second.has_specular()) - { - if (l->SpecularColor() != msgs::Convert(light.second.specular())) - { - l->SetSpecularColor(msgs::Convert(light.second.specular())); - } - } - if (!ignition::math::equal( - l->AttenuationRange(), - static_cast(light.second.range()))) - { - l->SetAttenuationRange(light.second.range()); - } - if (!ignition::math::equal( - l->AttenuationLinear(), - static_cast(light.second.attenuation_linear()))) - { - l->SetAttenuationLinear(light.second.attenuation_linear()); - } - if (!ignition::math::equal( - l->AttenuationConstant(), - static_cast(light.second.attenuation_constant()))) - { - l->SetAttenuationConstant(light.second.attenuation_constant()); - } - if (!ignition::math::equal( - l->AttenuationQuadratic(), - static_cast(light.second.attenuation_quadratic()))) - { - l->SetAttenuationQuadratic(light.second.attenuation_quadratic()); - } - if (l->CastShadows() != light.second.cast_shadows()) - l->SetCastShadows(light.second.cast_shadows()); - if (!ignition::math::equal( - l->Intensity(), - static_cast(light.second.intensity()))) - { - l->SetIntensity(light.second.intensity()); - } - auto lDirectional = - std::dynamic_pointer_cast(node); - if (lDirectional) - { - if (light.second.has_direction()) - { - if (lDirectional->Direction() != - msgs::Convert(light.second.direction())) - { - lDirectional->SetDirection( - msgs::Convert(light.second.direction())); - } - } - } - auto lSpotLight = - std::dynamic_pointer_cast(node); - if (lSpotLight) - { - if (light.second.has_direction()) - { - if (lSpotLight->Direction() != - msgs::Convert(light.second.direction())) - { - lSpotLight->SetDirection( - msgs::Convert(light.second.direction())); - } - } - if (lSpotLight->InnerAngle() != light.second.spot_inner_angle()) - lSpotLight->SetInnerAngle(light.second.spot_inner_angle()); - if (lSpotLight->OuterAngle() != light.second.spot_outer_angle()) - lSpotLight->SetOuterAngle(light.second.spot_outer_angle()); - if (!ignition::math::equal( - lSpotLight->Falloff(), - static_cast(light.second.spot_falloff()))) - { - lSpotLight->SetFalloff(light.second.spot_falloff()); - } - } - } - } - } + this->dataPtr->UpdateLights(entityLights); + // update entities' pose { IGN_PROFILE("RenderUtil::Update Poses"); @@ -1004,96 +1060,7 @@ void RenderUtil::Update() } else { - for (auto &it : actorAnimationData) - { - auto actorMesh = this->dataPtr->sceneManager.ActorMeshById(it.first); - auto actorVisual = this->dataPtr->sceneManager.NodeById(it.first); - auto actorSkel = this->dataPtr->sceneManager.ActorSkeletonById( - it.first); - if (!actorMesh || !actorVisual || !actorSkel) - { - ignerr << "Actor with Entity ID '" << it.first << "'. not found. " - << "Skipping skeleton animation update." << std::endl; - continue; - } - - AnimationUpdateData &animData = it.second; - if (!animData.valid) - { - ignerr << "invalid animation update data" << std::endl; - continue; - } - // Enable skeleton animation - if (!actorMesh->SkeletonAnimationEnabled(animData.animationName)) - { - // disable all animations for this actor - for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) - { - actorMesh->SetSkeletonAnimationEnabled( - actorSkel->Animation(i)->Name(), false, false, 0.0); - } - - // enable requested animation - actorMesh->SetSkeletonAnimationEnabled( - animData.animationName, true, animData.loop); - - // Set skeleton root node weight to zero so it is not affected by - // the animation being played. This is needed if trajectory animation - // is enabled. We need to let the trajectory animation set the - // position of the actor instead - common::SkeletonPtr skeleton = - this->dataPtr->sceneManager.ActorSkeletonById(it.first); - if (skeleton) - { - float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; - std::unordered_map weights; - weights[skeleton->RootNode()->Name()] = rootBoneWeight; - actorMesh->SetSkeletonWeights(weights); - } - } - // Update skeleton animation by setting animation time. - // Note that animation time is different from sim time. An actor can - // have multiple animations. Animation time is associated with - // current animation that is being played. It is also adjusted if - // interpotate_x is enabled. - actorMesh->UpdateSkeletonAnimation(animData.time); - - // manually update root transform in order to sync with trajectory - // animation - if (animData.followTrajectory) - { - common::SkeletonPtr skeleton = - this->dataPtr->sceneManager.ActorSkeletonById(it.first); - std::map rootTf; - rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; - actorMesh->SetSkeletonLocalTransforms(rootTf); - } - - // update actor trajectory animation - math::Pose3d globalPose; - if (entityPoses.find(it.first) != entityPoses.end()) - { - globalPose = entityPoses[it.first]; - } - - math::Pose3d trajPose; - // Trajectory from the ECS - if (trajectoryPoses.find(it.first) != trajectoryPoses.end()) - { - trajPose = trajectoryPoses[it.first]; - } - else - { - // trajectory from sdf script - common::PoseKeyFrame poseFrame(0.0); - if (animData.followTrajectory) - animData.trajectory.Waypoints()->InterpolatedKeyFrame(poseFrame); - trajPose.Pos() = poseFrame.Translation(); - trajPose.Rot() = poseFrame.Rotation(); - } - - actorVisual->SetLocalPose(trajPose + globalPose); - } + this->dataPtr->UpdateAnimation(actorAnimationData); } } @@ -1120,6 +1087,73 @@ void RenderUtil::Update() } } + // create new transparent visuals + { + for (const auto &link : newTransparentVisualLinks) + { + std::vector visEntities = + this->dataPtr->linkToVisualEntities[link]; + + for (const auto &visEntity : visEntities) + { + if (!this->dataPtr->viewingTransparent[visEntity]) + { + auto vis = this->dataPtr->sceneManager.VisualById(visEntity); + + this->dataPtr->sceneManager.UpdateTransparency(vis, + true /* transparent */); + this->dataPtr->viewingTransparent[visEntity] = true; + } + } + } + } + + // create new inertia visuals + { + for (const auto &link : newInertiaLinks) + { + // create a new id for the inertia visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->sceneManager.HasEntity(id) && + !this->dataPtr->viewingInertias[link]) + { + rendering::VisualPtr inrVisual = + this->dataPtr->sceneManager.CreateInertiaVisual( + id, this->dataPtr->entityInertials[link], link); + this->dataPtr->viewingInertias[link] = true; + this->dataPtr->linkToInertiaVisuals[link] = id; + break; + } + } + } + } + + // create new center of mass visuals + { + for (const auto &link : newCOMLinks) + { + // create a new id for the center of mass visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->sceneManager.HasEntity(id) && + !this->dataPtr->viewingCOM[link]) + { + rendering::VisualPtr inrVisual = + this->dataPtr->sceneManager.CreateCOMVisual( + id, this->dataPtr->entityInertials[link], link); + this->dataPtr->viewingCOM[link] = true; + this->dataPtr->linkToCOMVisuals[link] = id; + break; + } + } + } + } + // create new wireframe visuals { for (const auto &link : newWireframeVisualLinks) @@ -1176,60 +1210,24 @@ void RenderUtil::Update() } } - // update thermal camera - for (const auto &thermal : this->dataPtr->thermalCameraData) + this->dataPtr->UpdateThermalCamera(thermalCameraData); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateRenderingEntities( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); + auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, + Entity _parent, + const std::string &_topicSuffix) { - Entity id = thermal.first; - rendering::ThermalCameraPtr camera = - std::dynamic_pointer_cast( - this->dataPtr->sceneManager.NodeById(id)); - if (camera) - { - double resolution = std::get<0>(thermal.second); - - if (resolution > 0.0) - { - camera->SetLinearResolution(resolution); - } - else - { - ignwarn << "Unable to set thermal camera temperature linear resolution." - << " Value must be greater than 0. Using the default value: " - << camera->LinearResolution() << ". " << std::endl; - } - double minTemp = std::get<1>(thermal.second).min.Kelvin(); - double maxTemp = std::get<1>(thermal.second).max.Kelvin(); - if (maxTemp >= minTemp) - { - camera->SetMinTemperature(minTemp); - camera->SetMaxTemperature(maxTemp); - } - else - { - ignwarn << "Unable to set thermal camera temperature range." - << "Max temperature must be greater or equal to min. " - << "Using the default values : [" << camera->MinTemperature() - << ", " << camera->MaxTemperature() << "]." << std::endl; - } - } - } -} - -////////////////////////////////////////////////// -void RenderUtilPrivate::CreateRenderingEntities( - const EntityComponentManager &_ecm, const UpdateInfo &_info) -{ - IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); - auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, - Entity _parent, - const std::string &_topicSuffix) - { - sdf::Sensor sdfDataCopy(_sdfData); - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdfDataCopy.SetName(sensorScopedName); - // check topic - if (sdfDataCopy.Topic().empty()) + sdf::Sensor sdfDataCopy(_sdfData); + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdfDataCopy.SetName(sensorScopedName); + // check topic + if (sdfDataCopy.Topic().empty()) { sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); } @@ -1391,6 +1389,16 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); + // inertials + _ecm.Each( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + // collisions _ecm.Each( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + // collisions _ecm.EachNewremoveEntities[_entity] = _info.iterations; this->linkToVisualEntities.erase(_entity); this->linkToCollisionEntities.erase(_entity); + + if (this->linkToInertiaVisuals.find(_entity) != + this->linkToInertiaVisuals.end()) + { + this->removeEntities[this->linkToInertiaVisuals[_entity]] = + _info.iterations; + } + + if (this->linkToCOMVisuals.find(_entity) != + this->linkToCOMVisuals.end()) + { + this->removeEntities[this->linkToCOMVisuals[_entity]] = + _info.iterations; + } + + this->linkToInertiaVisuals.erase(_entity); + this->viewingInertias.erase(_entity); + this->linkToCOMVisuals.erase(_entity); + this->viewingCOM.erase(_entity); + this->entityInertials.erase(_entity); return true; }); @@ -2230,17 +2268,264 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node) } ///////////////////////////////////////////////// -void RenderUtil::ViewWireframes(const Entity &_entity) +void RenderUtilPrivate::RemoveSensor(const Entity _entity) { - std::vector visEntities; - std::vector links; + auto sensorEntityIt = this->sensorEntities.find(_entity); + if (sensorEntityIt != this->sensorEntities.end()) + { + this->removeSensorCb(_entity); + this->sensorEntities.erase(sensorEntityIt); + } +} - if (this->dataPtr->linkToVisualEntities.find(_entity) != - this->dataPtr->linkToVisualEntities.end()) +///////////////////////////////////////////////// +void RenderUtilPrivate::RemoveBoundingBox(const Entity _entity) +{ + auto wireBoxIt = this->wireBoxes.find(_entity); + if (wireBoxIt != this->wireBoxes.end()) { - visEntities = this->dataPtr->linkToVisualEntities[_entity]; + this->scene->DestroyVisual(wireBoxIt->second->Parent()); + this->wireBoxes.erase(wireBoxIt); + } +} + +///////////////////////////////////////////////// +void RenderUtilPrivate::UpdateLights( + const std::unordered_map &_entityLights) +{ + IGN_PROFILE("RenderUtil::Update Lights"); + for (const auto &light : _entityLights) + { + auto node = this->sceneManager.NodeById(light.first); + if (!node) + continue; + auto l = std::dynamic_pointer_cast(node); + if (l) + { + if (light.second.has_diffuse()) + { + if (l->DiffuseColor() != msgs::Convert(light.second.diffuse())) + l->SetDiffuseColor(msgs::Convert(light.second.diffuse())); + } + if (light.second.has_specular()) + { + if (l->SpecularColor() != msgs::Convert(light.second.specular())) + { + l->SetSpecularColor(msgs::Convert(light.second.specular())); + } + } + if (!ignition::math::equal( + l->AttenuationRange(), + static_cast(light.second.range()))) + { + l->SetAttenuationRange(light.second.range()); + } + if (!ignition::math::equal( + l->AttenuationLinear(), + static_cast(light.second.attenuation_linear()))) + { + l->SetAttenuationLinear(light.second.attenuation_linear()); + } + if (!ignition::math::equal( + l->AttenuationConstant(), + static_cast(light.second.attenuation_constant()))) + { + l->SetAttenuationConstant(light.second.attenuation_constant()); + } + if (!ignition::math::equal( + l->AttenuationQuadratic(), + static_cast(light.second.attenuation_quadratic()))) + { + l->SetAttenuationQuadratic(light.second.attenuation_quadratic()); + } + if (l->CastShadows() != light.second.cast_shadows()) + l->SetCastShadows(light.second.cast_shadows()); + auto lDirectional = + std::dynamic_pointer_cast(node); + if (lDirectional) + { + if (light.second.has_direction()) + { + if (lDirectional->Direction() != + msgs::Convert(light.second.direction())) + { + lDirectional->SetDirection( + msgs::Convert(light.second.direction())); + } + } + } + auto lSpotLight = + std::dynamic_pointer_cast(node); + if (lSpotLight) + { + if (light.second.has_direction()) + { + if (lSpotLight->Direction() != + msgs::Convert(light.second.direction())) + { + lSpotLight->SetDirection( + msgs::Convert(light.second.direction())); + } + } + if (lSpotLight->InnerAngle() != light.second.spot_inner_angle()) + lSpotLight->SetInnerAngle(light.second.spot_inner_angle()); + if (lSpotLight->OuterAngle() != light.second.spot_outer_angle()) + lSpotLight->SetOuterAngle(light.second.spot_outer_angle()); + if (!ignition::math::equal( + lSpotLight->Falloff(), + static_cast(light.second.spot_falloff()))) + { + lSpotLight->SetFalloff(light.second.spot_falloff()); + } + } + } + } +} + +///////////////////////////////////////////////// +void RenderUtilPrivate::UpdateThermalCamera(const std::unordered_map> &_thermalCamData) +{ + for (const auto &thermal : _thermalCamData) + { + Entity id = thermal.first; + rendering::ThermalCameraPtr camera = + std::dynamic_pointer_cast( + this->sceneManager.NodeById(id)); + if (camera) + { + double resolution = std::get<0>(thermal.second); + + if (resolution > 0.0) + { + camera->SetLinearResolution(resolution); + } + else + { + ignwarn << "Unable to set thermal camera temperature linear resolution." + << " Value must be greater than 0. Using the default value: " + << camera->LinearResolution() << ". " << std::endl; + } + double minTemp = std::get<1>(thermal.second).min.Kelvin(); + double maxTemp = std::get<1>(thermal.second).max.Kelvin(); + if (maxTemp >= minTemp) + { + camera->SetMinTemperature(minTemp); + camera->SetMaxTemperature(maxTemp); + } + else + { + ignwarn << "Unable to set thermal camera temperature range." + << "Max temperature must be greater or equal to min. " + << "Using the default values : [" << camera->MinTemperature() + << ", " << camera->MaxTemperature() << "]." << std::endl; + } + } + } +} + +///////////////////////////////////////////////// +void RenderUtilPrivate::UpdateAnimation( + const std::unordered_map &_actorAnimationData) +{ + for (auto &it : _actorAnimationData) + { + auto actorMesh = this->sceneManager.ActorMeshById(it.first); + auto actorVisual = this->sceneManager.NodeById(it.first); + auto actorSkel = this->sceneManager.ActorSkeletonById( + it.first); + if (!actorMesh || !actorVisual || !actorSkel) + { + ignerr << "Actor with Entity ID '" << it.first << "'. not found. " + << "Skipping skeleton animation update." << std::endl; + continue; + } + + const AnimationUpdateData &animData = it.second; + if (!animData.valid) + { + ignerr << "invalid animation update data" << std::endl; + continue; + } + // Enable skeleton animation + if (!actorMesh->SkeletonAnimationEnabled(animData.animationName)) + { + // disable all animations for this actor + for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) + { + actorMesh->SetSkeletonAnimationEnabled( + actorSkel->Animation(i)->Name(), false, false, 0.0); + } + + // enable requested animation + actorMesh->SetSkeletonAnimationEnabled( + animData.animationName, true, animData.loop); + + // Set skeleton root node weight to zero so it is not affected by + // the animation being played. This is needed if trajectory animation + // is enabled. We need to let the trajectory animation set the + // position of the actor instead + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + if (skeleton) + { + float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; + std::unordered_map weights; + weights[skeleton->RootNode()->Name()] = rootBoneWeight; + actorMesh->SetSkeletonWeights(weights); + } + } + // Update skeleton animation by setting animation time. + // Note that animation time is different from sim time. An actor can + // have multiple animations. Animation time is associated with + // current animation that is being played. It is also adjusted if + // interpotate_x is enabled. + actorMesh->UpdateSkeletonAnimation(animData.time); + + // manually update root transform in order to sync with trajectory + // animation + if (animData.followTrajectory) + { + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + std::map rootTf; + rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; + actorMesh->SetSkeletonLocalTransforms(rootTf); + } + + // update actor trajectory animation + math::Pose3d globalPose; + if (entityPoses.find(it.first) != entityPoses.end()) + { + globalPose = entityPoses[it.first]; + } + + math::Pose3d trajPose; + // Trajectory from the ECS + if (trajectoryPoses.find(it.first) != trajectoryPoses.end()) + { + trajPose = trajectoryPoses[it.first]; + } + else + { + // trajectory from sdf script + common::PoseKeyFrame poseFrame(0.0); + if (animData.followTrajectory) + animData.trajectory.Waypoints()->InterpolatedKeyFrame(poseFrame); + trajPose.Pos() = poseFrame.Translation(); + trajPose.Rot() = poseFrame.Rotation(); + } + + actorVisual->SetLocalPose(trajPose + globalPose); } - else if (this->dataPtr->modelToLinkEntities.find(_entity) != +} + +///////////////////////////////////////////////// +std::vector RenderUtil::FindChildLinks(const Entity &_entity) +{ + std::vector links; + + if (this->dataPtr->modelToLinkEntities.find(_entity) != this->dataPtr->modelToLinkEntities.end()) { links.insert(links.end(), @@ -2272,10 +2557,232 @@ void RenderUtil::ViewWireframes(const Entity &_entity) } } + return links; +} + +///////////////////////////////////////////////// +void RenderUtil::HideWireboxes(const Entity &_entity) +{ + if (this->dataPtr->wireBoxes.find(_entity) + != this->dataPtr->wireBoxes.end()) + { + ignition::rendering::WireBoxPtr wireBox = + this->dataPtr->wireBoxes[_entity]; + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(false); + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewInertia(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->dataPtr->entityInertials.find(_entity) != + this->dataPtr->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle inertia visuals + bool showInertia, showInertiaInit = false; + // first loop looks for new inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingInertias.find(inertiaLink) == + this->dataPtr->viewingInertias.end()) + { + this->dataPtr->newInertias.push_back(_entity); + showInertiaInit = showInertia = true; + } + } + + // second loop toggles already created inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingInertias.find(inertiaLink) == + this->dataPtr->viewingInertias.end()) + continue; + + // when viewing multiple inertias (e.g. _entity is a model), + // boolean for view inertias is based on first inrEntity in list + if (!showInertiaInit) + { + showInertia = !this->dataPtr->viewingInertias[inertiaLink]; + showInertiaInit = true; + } + + Entity inertiaVisualId = this->dataPtr->linkToInertiaVisuals[inertiaLink]; + rendering::VisualPtr inertiaVisual = + this->dataPtr->sceneManager.VisualById(inertiaVisualId); + if (inertiaVisual == nullptr) + { + ignerr << "Could not find inertia visual for entity [" << inertiaLink + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingInertias[inertiaLink] = showInertia; + inertiaVisual->SetVisible(showInertia); + + if (showInertia) + { + this->HideWireboxes(inertiaVisualId); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewCOM(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->dataPtr->entityInertials.find(_entity) != + this->dataPtr->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle center of mass visuals + bool showCOM, showCOMInit = false; + // first loop looks for new center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingCOM.find(inertiaLink) == + this->dataPtr->viewingCOM.end()) + { + this->dataPtr->newCOMVisuals.push_back(_entity); + showCOMInit = showCOM = true; + } + } + + // second loop toggles already created center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingCOM.find(inertiaLink) == + this->dataPtr->viewingCOM.end()) + continue; + + // when viewing multiple center of mass visuals (e.g. _entity is a model), + // boolean for view center of mass is based on first inertiaEntity in list + if (!showCOMInit) + { + showCOM = !this->dataPtr->viewingCOM[inertiaLink]; + showCOMInit = true; + } + + Entity comVisualId = this->dataPtr->linkToCOMVisuals[inertiaLink]; + rendering::VisualPtr comVisual = + this->dataPtr->sceneManager.VisualById(comVisualId); + if (comVisual == nullptr) + { + ignerr << "Could not find center of mass visual for entity [" + << inertiaLink + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingCOM[inertiaLink] = showCOM; + comVisual->SetVisible(showCOM); + + if (showCOM) + { + this->HideWireboxes(comVisualId); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewTransparent(const Entity &_entity) +{ + std::vector visEntities; + + if (this->dataPtr->linkToVisualEntities.find(_entity) != + this->dataPtr->linkToVisualEntities.end()) + { + visEntities = this->dataPtr->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + visEntities.insert(visEntities.end(), + this->dataPtr->linkToVisualEntities[link].begin(), + this->dataPtr->linkToVisualEntities[link].end()); + } + + // Toggle transparent mode + bool showTransparent, showTransparentInit = false; + + // first loop looks for new transparent entities + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingTransparent.find(visEntity) == + this->dataPtr->viewingTransparent.end()) + { + this->dataPtr->newTransparentEntities.push_back(_entity); + showTransparentInit = showTransparent = true; + } + } + + // second loop toggles transparent mode + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingTransparent.find(visEntity) == + this->dataPtr->viewingTransparent.end()) + continue; + + // when viewing multiple transparent visuals (e.g. _entity is a model), + // boolean for view as transparent is based on first visEntity in list + if (!showTransparentInit) + { + showTransparent = !this->dataPtr->viewingTransparent[visEntity]; + showTransparentInit = true; + } + + rendering::VisualPtr transparentVisual = + this->dataPtr->sceneManager.VisualById(visEntity); + if (transparentVisual == nullptr) + { + ignerr << "Could not find visual for entity [" << visEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingTransparent[visEntity] = showTransparent; + + this->dataPtr->sceneManager.UpdateTransparency(transparentVisual, + showTransparent); + + if (showTransparent) + { + // turn off wireboxes for visual entity + this->HideWireboxes(visEntity); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewWireframes(const Entity &_entity) +{ + std::vector visEntities; + + if (this->dataPtr->linkToVisualEntities.find(_entity) != + this->dataPtr->linkToVisualEntities.end()) + { + visEntities = this->dataPtr->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + for (const auto &link : links) + { visEntities.insert(visEntities.end(), this->dataPtr->linkToVisualEntities[link].begin(), this->dataPtr->linkToVisualEntities[link].end()); + } // Toggle wireframes bool showWireframe, showWireframeInit = false; @@ -2321,15 +2828,7 @@ void RenderUtil::ViewWireframes(const Entity &_entity) if (showWireframe) { // turn off wireboxes for visual entity - if (this->dataPtr->wireBoxes.find(visEntity) - != this->dataPtr->wireBoxes.end()) - { - ignition::rendering::WireBoxPtr wireBox = - this->dataPtr->wireBoxes[visEntity]; - auto visParent = wireBox->Parent(); - if (visParent) - visParent->SetVisible(false); - } + this->HideWireboxes(visEntity); } } } @@ -2338,53 +2837,26 @@ void RenderUtil::ViewWireframes(const Entity &_entity) void RenderUtil::ViewCollisions(const Entity &_entity) { std::vector colEntities; - std::vector links; if (this->dataPtr->linkToCollisionEntities.find(_entity) != this->dataPtr->linkToCollisionEntities.end()) { colEntities = this->dataPtr->linkToCollisionEntities[_entity]; } - else if (this->dataPtr->modelToLinkEntities.find(_entity) != - this->dataPtr->modelToLinkEntities.end()) - { - links.insert(links.end(), - this->dataPtr->modelToLinkEntities[_entity].begin(), - this->dataPtr->modelToLinkEntities[_entity].end()); - } - if (this->dataPtr->modelToModelEntities.find(_entity) != - this->dataPtr->modelToModelEntities.end()) - { - std::stack modelStack; - modelStack.push(_entity); - - std::vector childModels; - while (!modelStack.empty()) - { - Entity model = modelStack.top(); - modelStack.pop(); - - links.insert(links.end(), - this->dataPtr->modelToLinkEntities[model].begin(), - this->dataPtr->modelToLinkEntities[model].end()); - - childModels = this->dataPtr->modelToModelEntities[model]; - for (const auto &childModel : childModels) - { - modelStack.push(childModel); - } - } - } + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); for (const auto &link : links) + { colEntities.insert(colEntities.end(), this->dataPtr->linkToCollisionEntities[link].begin(), this->dataPtr->linkToCollisionEntities[link].end()); + } // create and/or toggle collision visuals - bool showCol, showColInit = false; + // first loop looks for new collisions for (const auto &colEntity : colEntities) { @@ -2425,16 +2897,7 @@ void RenderUtil::ViewCollisions(const Entity &_entity) if (showCol) { - // turn off wireboxes for collision entity - if (this->dataPtr->wireBoxes.find(colEntity) - != this->dataPtr->wireBoxes.end()) - { - ignition::rendering::WireBoxPtr wireBox = - this->dataPtr->wireBoxes[colEntity]; - auto visParent = wireBox->Parent(); - if (visParent) - visParent->SetVisible(false); - } + this->HideWireboxes(colEntity); } } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index a5d3503e57..a9c7e356a2 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -41,15 +41,18 @@ #include #include "ignition/rendering/Capsule.hh" +#include #include #include #include +#include #include #include #include #include #include #include +#include #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/Util.hh" @@ -95,6 +98,12 @@ class ignition::gazebo::SceneManagerPrivate /// \brief Map of sensor entity in Gazebo to sensor pointers. public: std::map sensors; + /// \brief The map of the original transparency values for the nodes. + public: std::map originalTransparency; + + /// \brief The map of the original depth write values for the nodes. + public: std::map originalDepthWrite; + /// \brief Helper function to compute actor trajectory at specified tiime /// \param[in] _id Actor entity's unique id /// \param[in] _time Simulation time @@ -1145,6 +1154,103 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, return light; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateInertiaVisual(Entity _id, + const math::Inertiald &_inertia, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::VisualPtr(); + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return rendering::VisualPtr(); + } + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + // It is possible to get here if the model entity is created then + // removed in between render updates. + return rendering::VisualPtr(); + } + parent = it->second; + } + + std::string name = std::to_string(_id); + if (parent) + name = parent->Name() + "::" + name; + + rendering::InertiaVisualPtr inertiaVisual = + this->dataPtr->scene->CreateInertiaVisual(name); + inertiaVisual->SetInertial(_inertia); + + rendering::VisualPtr inertiaVis = + std::dynamic_pointer_cast(inertiaVisual); + inertiaVis->SetUserData("gazebo-entity", static_cast(_id)); + inertiaVis->SetUserData("pause-update", static_cast(0)); + this->dataPtr->visuals[_id] = inertiaVis; + + if (parent) + { + inertiaVis->RemoveParent(); + parent->AddChild(inertiaVis); + } + return inertiaVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateCOMVisual(Entity _id, + const math::Inertiald &_inertia, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::VisualPtr(); + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return rendering::VisualPtr(); + } + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + // It is possible to get here if the model entity is created then + // removed in between render updates. + return rendering::VisualPtr(); + } + parent = it->second; + } + + if (!parent) + return rendering::VisualPtr(); + + std::string name = std::to_string(_id); + name = parent->Name() + "::" + name; + + rendering::COMVisualPtr comVisual = + this->dataPtr->scene->CreateCOMVisual(name); + comVisual->RemoveParent(); + parent->AddChild(comVisual); + comVisual->SetInertial(_inertia); + + rendering::VisualPtr comVis = + std::dynamic_pointer_cast(comVisual); + comVis->SetUserData("gazebo-entity", static_cast(_id)); + comVis->SetUserData("pause-update", static_cast(0)); + this->dataPtr->visuals[_id] = comVis; + + return comVis; +} + ///////////////////////////////////////////////// rendering::ParticleEmitterPtr SceneManager::CreateParticleEmitter( Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId) @@ -1593,6 +1699,19 @@ void SceneManager::RemoveEntity(Entity _id) auto it = this->dataPtr->visuals.find(_id); if (it != this->dataPtr->visuals.end()) { + // Remove visual's original transparency from map + rendering::VisualPtr vis = it->second; + this->dataPtr->originalTransparency.erase(vis->Name()); + // Remove visual's original depth write value from map + this->dataPtr->originalDepthWrite.erase(vis->Name()); + + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + auto geom = vis->GeometryByIndex(g); + this->dataPtr->originalTransparency.erase(geom->Name()); + this->dataPtr->originalDepthWrite.erase(geom->Name()); + } + this->dataPtr->scene->DestroyVisual(it->second); this->dataPtr->visuals.erase(it); return; @@ -1659,6 +1778,103 @@ rendering::NodePtr SceneManager::TopLevelNode( return node; } +//////////////////////////////////////////////// +void SceneManager::UpdateTransparency(const rendering::NodePtr &_node, + bool _makeTransparent) +{ + if (!_node) + return; + + for (auto n = 0u; n < _node->ChildCount(); ++n) + { + auto child = _node->ChildByIndex(n); + this->UpdateTransparency(child, _makeTransparent); + } + + auto vis = std::dynamic_pointer_cast(_node); + if (nullptr == vis) + return; + + // Visual material + auto visMat = vis->Material(); + if (nullptr != visMat) + { + auto visTransparency = + this->dataPtr->originalTransparency.find(vis->Name()); + auto visDepthWrite = + this->dataPtr->originalDepthWrite.find(vis->Name()); + if (_makeTransparent) + { + if (visTransparency == this->dataPtr->originalTransparency.end()) + { + this->dataPtr->originalTransparency[vis->Name()] = + visMat->Transparency(); + } + visMat->SetTransparency(1.0 - ((1.0 - visMat->Transparency()) * 0.5)); + + if (visDepthWrite == this->dataPtr->originalDepthWrite.end()) + { + this->dataPtr->originalDepthWrite[vis->Name()] = + visMat->DepthWriteEnabled(); + } + visMat->SetDepthWriteEnabled(false); + } + else + { + if (visTransparency != this->dataPtr->originalTransparency.end()) + { + visMat->SetTransparency(visTransparency->second); + } + if (visDepthWrite != this->dataPtr->originalDepthWrite.end()) + { + visMat->SetDepthWriteEnabled(visDepthWrite->second); + } + } + } + + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + auto geom = vis->GeometryByIndex(g); + + // Geometry material + auto geomMat = geom->Material(); + if (nullptr == geomMat || visMat == geomMat) + continue; + auto geomTransparency = + this->dataPtr->originalTransparency.find(geom->Name()); + auto geomDepthWrite = + this->dataPtr->originalDepthWrite.find(geom->Name()); + + if (_makeTransparent) + { + if (geomTransparency == this->dataPtr->originalTransparency.end()) + { + this->dataPtr->originalTransparency[geom->Name()] = + geomMat->Transparency(); + } + geomMat->SetTransparency(1.0 - ((1.0 - geomMat->Transparency()) * 0.5)); + + if (geomDepthWrite == this->dataPtr->originalDepthWrite.end()) + { + this->dataPtr->originalDepthWrite[geom->Name()] = + geomMat->DepthWriteEnabled(); + } + geomMat->SetDepthWriteEnabled(false); + } + else + { + if (geomTransparency != this->dataPtr->originalTransparency.end()) + { + geomMat->SetTransparency(geomTransparency->second); + } + if (geomDepthWrite != this->dataPtr->originalDepthWrite.end()) + { + geomMat->SetDepthWriteEnabled(geomDepthWrite->second); + } + } + } +} + ///////////////////////////////////////////////// AnimationUpdateData SceneManagerPrivate::ActorTrajectoryAt( Entity _id, diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 74b19a3ee3..530c3e3289 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -19,7 +19,6 @@ #include -#include #include #include #include @@ -28,6 +27,7 @@ #include #include #include +#include #include #include @@ -38,8 +38,6 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" -#include "SpeedLimiter.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -164,10 +162,10 @@ class ignition::gazebo::systems::AckermannSteeringPrivate public: std::chrono::steady_clock::duration lastOdomTime{0}; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -258,56 +256,48 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; - // Parse speed limiter parameters. - bool hasVelocityLimits = false; - bool hasAccelerationLimits = false; - bool hasJerkLimits = false; - double minVel = std::numeric_limits::lowest(); - double maxVel = std::numeric_limits::max(); - double minAccel = std::numeric_limits::lowest(); - double maxAccel = std::numeric_limits::max(); - double minJerk = std::numeric_limits::lowest(); - double maxJerk = std::numeric_limits::max(); + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) { - minVel = _sdf->Get("min_velocity"); - hasVelocityLimits = true; + const double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); } if (_sdf->HasElement("max_velocity")) { - maxVel = _sdf->Get("max_velocity"); - hasVelocityLimits = true; + const double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } if (_sdf->HasElement("min_acceleration")) { - minAccel = _sdf->Get("min_acceleration"); - hasAccelerationLimits = true; + const double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } if (_sdf->HasElement("max_acceleration")) { - maxAccel = _sdf->Get("max_acceleration"); - hasAccelerationLimits = true; + const double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); } if (_sdf->HasElement("min_jerk")) { - minJerk = _sdf->Get("min_jerk"); - hasJerkLimits = true; + const double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); } if (_sdf->HasElement("max_jerk")) { - maxJerk = _sdf->Get("max_jerk"); - hasJerkLimits = true; + const double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - - this->dataPtr->limiterAng = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) @@ -696,11 +686,11 @@ void AckermannSteeringPrivate::UpdateVelocity( angVel = this->targetVel.angular().z(); } - const double dt = std::chrono::duration(_info.dt).count(); - // Limit the target velocity if needed. - this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); - this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); // Update history of commands. this->last1Cmd = last0Cmd; diff --git a/src/systems/ackermann_steering/CMakeLists.txt b/src/systems/ackermann_steering/CMakeLists.txt index 69af11afea..d797c6fe20 100644 --- a/src/systems/ackermann_steering/CMakeLists.txt +++ b/src/systems/ackermann_steering/CMakeLists.txt @@ -1,7 +1,6 @@ gz_add_system(ackermann-steering SOURCES AckermannSteering.cc - SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/ackermann_steering/SpeedLimiter.cc b/src/systems/ackermann_steering/SpeedLimiter.cc deleted file mode 100644 index ebae2b5b81..0000000000 --- a/src/systems/ackermann_steering/SpeedLimiter.cc +++ /dev/null @@ -1,209 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * 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 PAL Robotics 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. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#include - -#include "SpeedLimiter.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -/// \brief Private SpeedLimiter data class. -class ignition::gazebo::systems::SpeedLimiterPrivate -{ - /// \brief Class constructor. - /// \param [in] _hasVelocityLimits if true, applies velocity limits. - /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. - /// \param [in] _hasJerkLimits if true, applies jerk limits. - /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. - /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. - /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. - /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. - /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. - /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. - public: SpeedLimiterPrivate(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : hasVelocityLimits(_hasVelocityLimits), - hasAccelerationLimits(_hasAccelerationLimits), - hasJerkLimits(_hasJerkLimits), - minVelocity(_minVelocity), - maxVelocity(_maxVelocity), - minAcceleration(_minAcceleration), - maxAcceleration(_maxAcceleration), - minJerk(_minJerk), - maxJerk(_maxJerk) - { - } - - /// \brief Enable/Disable velocity. - public: bool hasVelocityLimits; - - /// \brief Enable/Disable acceleration. - public: bool hasAccelerationLimits; - - /// \brief Enable/Disable jerk. - public: bool hasJerkLimits; - - /// \brief Minimum velocity limit. - public: double minVelocity; - - /// \brief Maximum velocity limit. - public: double maxVelocity; - - /// \brief Minimum acceleration limit. - public: double minAcceleration; - - /// \brief Maximum acceleration limit. - public: double maxAcceleration; - - /// \brief Minimum jerk limit. - public: double minJerk; - - /// \brief Maximum jerk limit. - public: double maxJerk; -}; - -////////////////////////////////////////////////// -SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : dataPtr(std::make_unique(_hasVelocityLimits, - _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, - _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) -{ -} - -////////////////////////////////////////////////// -SpeedLimiter::~SpeedLimiter() -{ -} - -////////////////////////////////////////////////// -double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const -{ - const double tmp = _v; - - this->LimitJerk(_v, _v0, _v1, _dt); - this->LimitAcceleration(_v, _v0, _dt); - this->LimitVelocity(_v); - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitVelocity(double &_v) const -{ - const double tmp = _v; - - if (this->dataPtr->hasVelocityLimits) - { - _v = ignition::math::clamp( - _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const -{ - const double tmp = _v; - - if (this->dataPtr->hasAccelerationLimits) - { - const double dvMin = this->dataPtr->minAcceleration * _dt; - const double dvMax = this->dataPtr->maxAcceleration * _dt; - - const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); - - _v = _v0 + dv; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) - const -{ - const double tmp = _v; - - if (this->dataPtr->hasJerkLimits) - { - const double dv = _v - _v0; - const double dv0 = _v0 - _v1; - - const double dt2 = 2. * _dt * _dt; - - const double daMin = this->dataPtr->minJerk * dt2; - const double daMax = this->dataPtr->maxJerk * dt2; - - const double da = ignition::math::clamp(dv - dv0, daMin, daMax); - - _v = _v0 + dv0 + da; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index cadc7b4b3d..26d8d178a4 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -232,38 +232,7 @@ void CameraVideoRecorderPrivate::OnPostRender() // get scene if (!this->scene) { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - igndbg << "More than one engine is available. " - << "Camera video recorder system will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - ignerr << "Internal error: failed to load engine [" << engineName - << "]. Camera video recorder system won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto s = engine->SceneByIndex(0); - if (!s) - { - ignerr << "Internal error: scene is null." << std::endl; - return; - } - this->scene = s; + this->scene = rendering::sceneFromFirstRenderEngine(); } // return if scene not ready or no sensors available. diff --git a/src/systems/diff_drive/CMakeLists.txt b/src/systems/diff_drive/CMakeLists.txt index 0a415da6d4..7c7da3ec05 100644 --- a/src/systems/diff_drive/CMakeLists.txt +++ b/src/systems/diff_drive/CMakeLists.txt @@ -1,7 +1,6 @@ gz_add_system(diff-drive SOURCES DiffDrive.cc - SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index ef73b4bc95..1df398e07e 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -38,8 +39,6 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" -#include "SpeedLimiter.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -129,10 +128,10 @@ class ignition::gazebo::systems::DiffDrivePrivate public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -206,56 +205,48 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; - // Parse speed limiter parameters. - bool hasVelocityLimits = false; - bool hasAccelerationLimits = false; - bool hasJerkLimits = false; - double minVel = std::numeric_limits::lowest(); - double maxVel = std::numeric_limits::max(); - double minAccel = std::numeric_limits::lowest(); - double maxAccel = std::numeric_limits::max(); - double minJerk = std::numeric_limits::lowest(); - double maxJerk = std::numeric_limits::max(); + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) { - minVel = _sdf->Get("min_velocity"); - hasVelocityLimits = true; + const double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); } if (_sdf->HasElement("max_velocity")) { - maxVel = _sdf->Get("max_velocity"); - hasVelocityLimits = true; + const double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } if (_sdf->HasElement("min_acceleration")) { - minAccel = _sdf->Get("min_acceleration"); - hasAccelerationLimits = true; + const double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } if (_sdf->HasElement("max_acceleration")) { - maxAccel = _sdf->Get("max_acceleration"); - hasAccelerationLimits = true; + const double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); } if (_sdf->HasElement("min_jerk")) { - minJerk = _sdf->Get("min_jerk"); - hasJerkLimits = true; + const double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); } if (_sdf->HasElement("max_jerk")) { - maxJerk = _sdf->Get("max_jerk"); - hasJerkLimits = true; + const double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - - this->dataPtr->limiterAng = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) @@ -563,11 +554,11 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, angVel = this->targetVel.angular().z(); } - const double dt = std::chrono::duration(_info.dt).count(); - // Limit the target velocity if needed. - this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); - this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); // Update history of commands. this->last1Cmd = last0Cmd; diff --git a/src/systems/diff_drive/SpeedLimiter.cc b/src/systems/diff_drive/SpeedLimiter.cc deleted file mode 100644 index 59b50f54b9..0000000000 --- a/src/systems/diff_drive/SpeedLimiter.cc +++ /dev/null @@ -1,200 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * 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 PAL Robotics 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. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#include - -#include "SpeedLimiter.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -/// \brief Private SpeedLimiter data class. -class ignition::gazebo::systems::SpeedLimiterPrivate -{ - /// \brief Class constructor. - public: SpeedLimiterPrivate(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : hasVelocityLimits(_hasVelocityLimits), - hasAccelerationLimits(_hasAccelerationLimits), - hasJerkLimits(_hasJerkLimits), - minVelocity(_minVelocity), - maxVelocity(_maxVelocity), - minAcceleration(_minAcceleration), - maxAcceleration(_maxAcceleration), - minJerk(_minJerk), - maxJerk(_maxJerk) - { - } - - /// \brief Enable/Disable velocity. - public: bool hasVelocityLimits; - - /// \brief Enable/Disable acceleration. - public: bool hasAccelerationLimits; - - /// \brief Enable/Disable jerk. - public: bool hasJerkLimits; - - /// \brief Minimum velocity limit. - public: double minVelocity; - - /// \brief Maximum velocity limit. - public: double maxVelocity; - - /// \brief Minimum acceleration limit. - public: double minAcceleration; - - /// \brief Maximum acceleration limit. - public: double maxAcceleration; - - /// \brief Minimum jerk limit. - public: double minJerk; - - /// \brief Maximum jerk limit. - public: double maxJerk; -}; - -////////////////////////////////////////////////// -SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : dataPtr(std::make_unique(_hasVelocityLimits, - _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, - _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) -{ -} - -////////////////////////////////////////////////// -SpeedLimiter::~SpeedLimiter() -{ -} - -////////////////////////////////////////////////// -double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const -{ - const double tmp = _v; - - this->LimitJerk(_v, _v0, _v1, _dt); - this->LimitAcceleration(_v, _v0, _dt); - this->LimitVelocity(_v); - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitVelocity(double &_v) const -{ - const double tmp = _v; - - if (this->dataPtr->hasVelocityLimits) - { - _v = ignition::math::clamp( - _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const -{ - const double tmp = _v; - - if (this->dataPtr->hasAccelerationLimits) - { - const double dvMin = this->dataPtr->minAcceleration * _dt; - const double dvMax = this->dataPtr->maxAcceleration * _dt; - - const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); - - _v = _v0 + dv; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) - const -{ - const double tmp = _v; - - if (this->dataPtr->hasJerkLimits) - { - const double dv = _v - _v0; - const double dv0 = _v0 - _v1; - - const double dt2 = 2. * _dt * _dt; - - const double daMin = this->dataPtr->minJerk * dt2; - const double daMax = this->dataPtr->maxJerk * dt2; - - const double da = ignition::math::clamp(dv - dv0, daMin, daMax); - - _v = _v0 + dv0 + da; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} diff --git a/src/systems/diff_drive/SpeedLimiter.hh b/src/systems/diff_drive/SpeedLimiter.hh deleted file mode 100644 index 34286bc3ae..0000000000 --- a/src/systems/diff_drive/SpeedLimiter.hh +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * 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 PAL Robotics 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. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ - -#include - -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // Forward declaration. - class SpeedLimiterPrivate; - - /// \brief Class to limit velocity, acceleration and jerk. - /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller - class SpeedLimiter - { - /// \brief Constructor. - /// \param [in] _hasVelocityLimits if true, applies velocity limits. - /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. - /// \param [in] _hasJerkLimits if true, applies jerk limits. - /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. - /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. - /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. - /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. - /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. - /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. - public: SpeedLimiter(bool _hasVelocityLimits = false, - bool _hasAccelerationLimits = false, - bool _hasJerkLimits = false, - double _minVelocity = 0.0, - double _maxVelocity = 0.0, - double _minAcceleration = 0.0, - double _maxAcceleration = 0.0, - double _minJerk = 0.0, - double _maxJerk = 0.0); - - /// \brief Destructor. - public: ~SpeedLimiter(); - - /// \brief Limit the velocity and acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double Limit(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Limit the velocity. - /// \param [in, out] _v Velocity [m/s]. - /// \return Limiting factor (1.0 if none). - public: double LimitVelocity(double &_v) const; - - /// \brief Limit the acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double LimitAcceleration(double &_v, - double _v0, - double _dt) const; - - /// \brief Limit the jerk. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. - public: double LimitJerk(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; - }; - } -} -} -} - -#endif diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 60b1ab327e..3726eb69e9 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include @@ -164,9 +165,14 @@ void KineticEnergyMonitor::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/, +void KineticEnergyMonitor::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { + IGN_PROFILE("KineticEnergyMonitor::PostUpdate"); + // Nothing left to do if paused or the publisher wasn't created. + if (_info.paused || !this->dataPtr->pub) + return; + if (this->dataPtr->linkEntity != kNullEntity) { Link link(this->dataPtr->linkEntity); @@ -191,10 +197,10 @@ void KineticEnergyMonitor::PostUpdate(const UpdateInfo &/*_info*/, } } -IGNITION_ADD_PLUGIN(KineticEnergyMonitor, System, - KineticEnergyMonitor::ISystemConfigure, - KineticEnergyMonitor::ISystemPostUpdate -) +IGNITION_ADD_PLUGIN(KineticEnergyMonitor, + ignition::gazebo::System, + KineticEnergyMonitor::ISystemConfigure, + KineticEnergyMonitor::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(KineticEnergyMonitor, "ignition::gazebo::systems::KineticEnergyMonitor") diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index 25fa70239f..fa3b87bbd7 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -97,7 +97,7 @@ namespace systems \endverbatim */ - class KineticEnergyMonitor: + class KineticEnergyMonitor : public System, public ISystemConfigure, public ISystemPostUpdate diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc index 8885973829..492666b0ec 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -55,8 +56,8 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: void Load(const EntityComponentManager &_ecm); /// \brief Actual function that enables the plugin. - /// \param[in] _value True to enable plugin. - public: void Enable(const bool _value); + /// \param[in] _req Whether to enable the plugin or disable it. + public: void Enable(const ignition::msgs::Boolean &_req); /// \brief Callback for the depth camera /// \param[in] _msg Message from the subscribed topic @@ -98,6 +99,9 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Whether to visualize the normal forces. public: bool visualizeForces{false}; + /// \brief Whether to visualize the contacts. + public: bool visualizeContacts{false}; + /// \brief Model interface. public: Model model{kNullEntity}; @@ -112,7 +116,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: ignition::gazebo::Entity objectCollisionEntity; /// \brief Whether the plugin is enabled. - public: bool enabled{true}; + public: std::atomic enabled{true}; /// \brief Initialization flag. public: bool initialized{false}; @@ -150,7 +154,7 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate public: bool visualizeSensor{false}; /// \brief Size of the contact sensor - public: ignition::math::Vector3d sensorSize; + public: ignition::math::Vector3d sensorSize{0.005, 0.02, 0.02}; /// \brief Extended sensing distance. The sensor will output data coming from /// its volume plus this distance. @@ -164,6 +168,12 @@ class ignition::gazebo::systems::OpticalTactilePluginPrivate /// \brief Flag for allowing the plugin to output error/warning only once public: bool initErrorPrinted{false}; + + /// \brief Normal forces publisher + public: ignition::transport::Node::Publisher normalForcesPub; + + /// \brief Namespace for transport topics + public: std::string ns{"optical_tactile_sensor"}; }; ////////////////////////////////////////////////// @@ -228,6 +238,16 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->visualizeForces = _sdf->Get("visualize_forces"); } + if (!_sdf->HasElement("visualize_contacts")) + { + igndbg << "Missing parameter , " + << "setting to " << this->dataPtr->visualizeContacts << std::endl; + } + else + { + this->dataPtr->visualizeContacts = _sdf->Get("visualize_contacts"); + } + if (!_sdf->HasElement("extended_sensing")) { igndbg << "Missing parameter , " @@ -273,6 +293,73 @@ void OpticalTactilePlugin::Configure(const Entity &_entity, this->dataPtr->forceLength = _sdf->Get("force_length"); } } + + if (!_sdf->HasElement("namespace")) + { + igndbg << "Missing parameter , " + << "setting to " << this->dataPtr->ns << std::endl; + } + else + { + this->dataPtr->ns = _sdf->Get("namespace"); + } + + // Get the size of the sensor from the SDF + // If there's no specified inside , a default one + // is set + if (_sdf->GetParent() != nullptr) + { + if (_sdf->GetParent()->GetElement("link") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision") + != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry") != nullptr) + { + if (_sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box") != nullptr) + { + this->dataPtr->sensorSize = + _sdf->GetParent()->GetElement("link")->GetElement("collision")-> + GetElement("geometry")->GetElement("box")-> + Get("size"); + igndbg << "Setting sensor size to box collision size: [" + << this->dataPtr->sensorSize << "]" << std::endl; + } + } + } + } + } + + // Advertise topic for normal forces + std::string normalForcesTopic = "/" + this->dataPtr->ns + "/normal_forces"; + this->dataPtr->normalForcesPub = + this->dataPtr->node.Advertise(normalForcesTopic); + if (!this->dataPtr->normalForcesPub) + { + ignerr << "Error advertising topic [" << normalForcesTopic << "]" + << std::endl; + } + else + { + ignmsg << "Topic publishing normal forces [" << normalForcesTopic << "]" + << std::endl; + } + + // Advertise enabling service + std::string enableService = "/" + this->dataPtr->ns + "/enable"; + if (!this->dataPtr->node.Advertise(enableService, + &OpticalTactilePluginPrivate::Enable, this->dataPtr.get())) + { + ignerr << "Error advertising service [" << enableService << "]" + << std::endl; + } + else + { + ignmsg << "Service to enable tactile sensor [" << enableService << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -282,7 +369,7 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, IGN_PROFILE("OpticalTactilePlugin::PreUpdate"); // Nothing left to do if paused - if (_info.paused) + if (_info.paused || !this->dataPtr->enabled) return; if (!this->dataPtr->initialized) @@ -327,15 +414,26 @@ void OpticalTactilePlugin::PreUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void OpticalTactilePlugin::PostUpdate( const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) + const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("OpticalTactilePlugin::PostUpdate"); // Nothing left to do if paused or failed to initialize. - if (_info.paused || !this->dataPtr->initialized) + if (_info.paused || !this->dataPtr->initialized || !this->dataPtr->enabled) return; // TODO(anyone) Get ContactSensor data and merge it with DepthCamera data + if (this->dataPtr->visualizeContacts) + { + auto *contacts = + _ecm.Component( + this->dataPtr->sensorCollisionEntity); + + if (nullptr != contacts) + { + this->dataPtr->visualizePtr->RequestContactsMarkerMsg(contacts); + } + } // Process camera message if it's new { @@ -524,9 +622,20 @@ void OpticalTactilePluginPrivate::Load(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void OpticalTactilePluginPrivate::Enable(const bool /*_value*/) +void OpticalTactilePluginPrivate::Enable(const ignition::msgs::Boolean &_req) { - // todo(mcres) Implement method + if (_req.data() != this->enabled) + { + ignmsg << "Enabling optical tactile sensor with namespace [" << this->ns + << "]: " << _req.data() << std::endl; + } + + this->enabled = _req.data(); + + if (!_req.data()) + { + this->visualizePtr->RemoveNormalForcesAndContactsMarkers(); + } } ////////////////////////////////////////////////// @@ -535,7 +644,7 @@ void OpticalTactilePluginPrivate::DepthCameraCallback( { // This check avoids running the callback at t=0 and getting // unexpected markers in the scene - if (!this->initialized) + if (!this->initialized || !this->enabled) return; // Check whether DepthCamera returns FLOAT32 data @@ -652,6 +761,17 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // Declare variables for storing the XYZ points ignition::math::Vector3f p1, p2, p3, p4, markerPosition; + // Message for publishing normal forces + ignition::msgs::Image normalsMsg; + normalsMsg.set_width(_msg.width()); + normalsMsg.set_height(_msg.height()); + normalsMsg.set_step(3 * sizeof(float) * _msg.width()); + normalsMsg.set_pixel_format_type(ignition::msgs::PixelFormatType::R_FLOAT32); + + uint32_t bufferSize = 3 * sizeof(float) * _msg.width() * _msg.height(); + std::shared_ptr normalForcesBuffer(new char[bufferSize]); + uint32_t bufferIndex; + // Marker messages representing the normal forces ignition::msgs::Marker positionMarkerMsg; ignition::msgs::Marker forceMarkerMsg; @@ -682,8 +802,15 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( // https://github.com/ignitionrobotics/ign-math/issues/144 ignition::math::Vector3f normalForce = direction.Normalized(); - // todo(mcres) Normal forces are computed even if visualization - // is turned off. These forces should be published in the future. + // Add force to buffer + // Forces buffer is composed of XYZ coordinates, while _msg buffer is + // made up of XYZRGB values + bufferIndex = j * (_msg.row_step() / 2) + i * (_msg.point_step() / 2); + normalForcesBuffer.get()[bufferIndex] = normalForce.X(); + normalForcesBuffer.get()[bufferIndex + sizeof(float)] = normalForce.Y(); + normalForcesBuffer.get()[bufferIndex + 2 * sizeof(float)] = + normalForce.Z(); + if (!_visualizeForces) continue; @@ -694,6 +821,11 @@ void OpticalTactilePluginPrivate::ComputeNormalForces( } } + // Publish message + normalsMsg.set_data(normalForcesBuffer.get(), + 3 * sizeof(float) * _msg.width() * _msg.height()); + this->normalForcesPub.Publish(normalsMsg); + if (_visualizeForces) { this->visualizePtr->RequestNormalForcesMarkerMsgs(positionMarkerMsg, diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index db19bb09ff..687246b6a8 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -31,73 +31,95 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { - // Forward declaration - class OpticalTactilePluginPrivate; + // Forward declaration + class OpticalTactilePluginPrivate; - /// \brief Plugin that implements an optical tactile sensor - /// - /// It requires that contact sensor and depth camera be placed in at least - /// one link on the model on which this plugin is attached. - /// - /// Parameters: - /// - /// (todo) Set this to true so the plugin works from the start and - /// doesn't need to be enabled. This element is optional, and the - /// default value is true. - /// - /// Number n of pixels to skip when visualizing - /// the forces. One vector representing a normal force is computed for - /// every nth pixel. This element must be positive and it is optional. - /// The default value is 30 - /// - /// Set this to true so the plugin visualizes the normal - /// forces in the 3D world. This element is optional, and the - /// default value is false. - /// - /// Length in meters of the forces visualized if - /// is set to true. This parameter is optional, and the - /// default value is 0.01. - /// - /// Extended sensing distance in meters. The sensor will - /// output data coming from its collision geometry plus this distance. This - /// element is optional, and the default value is 0.001. - /// - /// Whether to visualize the sensor or not. This element - /// is optional, and the default value is false. + /// \brief Plugin that implements an optical tactile sensor + /// + /// It requires that contact sensor and depth camera be placed in at least + /// one link on the model on which this plugin is attached. + /// TODO: + /// Currently, the contacts returned from the physics engine (which tends to + /// be sparse) and the normal forces separately computed from the depth + /// camera (which is dense, resolution adjustable) are disjoint. It is + /// left as future work to combine the two sets of data. + /// + /// Parameters: + /// + /// Set this to true so the plugin works from the start and + /// doesn't need to be enabled. This element is optional, and the + /// default value is true. + /// + /// Namespace for transport topics/services. If there are more + /// than one optical tactile plugins, their namespaces should + /// be different. + /// This element is optional, and the default value is + /// "optical_tactile_sensor". + /// //enable : Service used to enable and disable the + /// plugin. + /// //normal_forces : Topic where a message is + /// published each time the + /// normal forces are computed + /// + /// Number n of pixels to skip when visualizing + /// the forces. One vector representing a normal + /// force is computed for every nth pixel. This + /// element must be positive and it is optional. + /// The default value is 30. + /// + /// Length in meters of the forces visualized if + /// is set to true. This parameter is + /// optional, and the default value is 0.01. + /// + /// Extended sensing distance in meters. The sensor will + /// output data coming from its collision geometry plus + /// this distance. This element is optional, and the + /// default value is 0.001. + /// + /// Whether to visualize the sensor or not. This element + /// is optional, and the default value is false. + /// + /// Whether to visualize the contacts from the contact + /// sensor based on physics. This element is optional, + /// and the default value is false. + /// + /// Whether to visualize normal forces computed from the + /// depth camera. This element is optional, and the + /// default value is false. - class OpticalTactilePlugin : - public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - { - /// \brief Constructor - public: OpticalTactilePlugin(); + class OpticalTactilePlugin : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: OpticalTactilePlugin(); - /// \brief Destructor - public: ~OpticalTactilePlugin() override = default; + /// \brief Destructor + public: ~OpticalTactilePlugin() override = default; - // Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &_eventMgr) override; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; - /// Documentation inherited - public: void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; - // Documentation inherited - public: void PostUpdate( + // Documentation inherited + public: void PostUpdate( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) override; - /// \brief Private data pointer - private: std::unique_ptr dataPtr; - }; -} -} -} -} + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc index f18fdb9d32..5384b3006e 100644 --- a/src/systems/optical_tactile_plugin/Visualization.cc +++ b/src/systems/optical_tactile_plugin/Visualization.cc @@ -15,21 +15,16 @@ * */ +#include +#include #include #include "Visualization.hh" -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ -namespace systems -{ -namespace optical_tactile_sensor -{ +using namespace ignition; +using namespace gazebo; +using namespace systems; +using namespace optical_tactile_sensor; ////////////////////////////////////////////////// OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( @@ -52,8 +47,10 @@ OpticalTactilePluginVisualization::OpticalTactilePluginVisualization( void OpticalTactilePluginVisualization::InitializeSensorMarkerMsg( ignition::msgs::Marker &_sensorMarkerMsg) { - // Initialize the marker for visualizing the sensor as a grey transparent box + // Reset all fields + _sensorMarkerMsg = ignition::msgs::Marker(); + // Initialize the marker for visualizing the sensor as a grey transparent box _sensorMarkerMsg.set_ns("sensor_" + this->modelName); _sensorMarkerMsg.set_id(1); _sensorMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); @@ -85,16 +82,73 @@ void OpticalTactilePluginVisualization::RequestSensorMarkerMsg( this->node.Request("/marker", sensorMarkerMsg); } +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::InitializeContactsMarkerMsg( + ignition::msgs::Marker &_contactsMarkerMsg) +{ + // Reset all fields + _contactsMarkerMsg = ignition::msgs::Marker(); + + // Initialize the marker for visualizing the physical contacts as red lines + _contactsMarkerMsg.set_ns("contacts_" + this->modelName); + _contactsMarkerMsg.set_id(1); + _contactsMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + _contactsMarkerMsg.set_type(ignition::msgs::Marker::LINE_LIST); + _contactsMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + + ignition::msgs::Set(_contactsMarkerMsg.mutable_material()-> + mutable_ambient(), math::Color(1, 0, 0, 1)); + ignition::msgs::Set(_contactsMarkerMsg.mutable_material()-> + mutable_diffuse(), math::Color(1, 0, 0, 1)); + _contactsMarkerMsg.mutable_lifetime()->set_sec(1); +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::AddContactToMarkerMsg( + ignition::msgs::Contact const &_contact, + ignition::msgs::Marker &_contactMarkerMsg) +{ + // todo(anyone) once available, use normal field in the Contact message + ignition::math::Vector3d contactNormal(0, 0, 0.03); + + // For each contact, add a line marker starting from the contact position, + // ending at the endpoint of the normal. + for (auto const &position : _contact.position()) + { + ignition::math::Vector3d startPoint = ignition::msgs::Convert(position); + ignition::math::Vector3d endPoint = startPoint + contactNormal; + + ignition::msgs::Set(_contactMarkerMsg.add_point(), startPoint); + ignition::msgs::Set(_contactMarkerMsg.add_point(), endPoint); + } +} + +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RequestContactsMarkerMsg( + const components::ContactSensorData *_contacts) +{ + ignition::msgs::Marker contactsMarkerMsg; + this->InitializeContactsMarkerMsg(contactsMarkerMsg); + + for (const auto &contact : _contacts->Data().contact()) + { + this->AddContactToMarkerMsg(contact, contactsMarkerMsg); + } + + this->node.Request("/marker", contactsMarkerMsg); +} + ////////////////////////////////////////////////// void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg) { - // Initialize marker messages for position and force of the contacts + _positionMarkerMsg = ignition::msgs::Marker(); + _forceMarkerMsg = ignition::msgs::Marker(); - // Blue points for positions - // Green lines for forces + // Initialize marker messages for position and force of the contacts + // Positions computed from camera _positionMarkerMsg.set_ns("positions_" + this->modelName); _positionMarkerMsg.set_id(1); _positionMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); @@ -110,6 +164,7 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( _forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); // Set material properties and lifetime + // Blue points for positions ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> mutable_ambient(), math::Color(0, 0, 1, 1)); ignition::msgs::Set(_positionMarkerMsg.mutable_material()-> @@ -117,6 +172,7 @@ void OpticalTactilePluginVisualization::InitializeNormalForcesMarkerMsgs( _positionMarkerMsg.mutable_lifetime()->set_nsec( static_cast(this->cameraUpdateRate * 1000000000)); + // Green lines for forces ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> mutable_ambient(), math::Color(0, 1, 0, 1)); ignition::msgs::Set(_forceMarkerMsg.mutable_material()-> @@ -157,12 +213,12 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; normalForcePoseFromWorld.Correct(); - // Get the first point of the normal force - ignition::math::Vector3f firstPointFromWorld = + // Get the start point of the normal force + ignition::math::Vector3f startPointFromWorld = normalForcePoseFromWorld.Pos(); // Move the normal force pose a distance of forceLength along the direction - // of _normalForce and get the second point + // of _normalForce and get the end point normalForcePoseFromSensor.Set(normalForcePositionFromSensor + _normalForce * this->forceLength, normalForceOrientationFromSensor); @@ -170,27 +226,25 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs( (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose; normalForcePoseFromWorld.Correct(); - ignition::math::Vector3f secondPointFromWorld = + ignition::math::Vector3f endPointFromWorld = normalForcePoseFromWorld.Pos(); // Check invalid points to avoid data transfer overhead - if (firstPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0) || - secondPointFromWorld == ignition::math::Vector3f(0.0, 0.0, 0.0)) + if (abs(startPointFromWorld.Distance(endPointFromWorld)) < 1e-6) return; - // Add points to the messages - + // Position ignition::msgs::Set(_positionMarkerMsg.add_point(), - ignition::math::Vector3d(firstPointFromWorld.X(), - firstPointFromWorld.Y(), firstPointFromWorld.Z())); + ignition::math::Vector3d(startPointFromWorld.X(), + startPointFromWorld.Y(), startPointFromWorld.Z())); + // Normal ignition::msgs::Set(_forceMarkerMsg.add_point(), - ignition::math::Vector3d(firstPointFromWorld.X(), - firstPointFromWorld.Y(), firstPointFromWorld.Z())); - + ignition::math::Vector3d(startPointFromWorld.X(), + startPointFromWorld.Y(), startPointFromWorld.Z())); ignition::msgs::Set(_forceMarkerMsg.add_point(), - ignition::math::Vector3d(secondPointFromWorld.X(), - secondPointFromWorld.Y(), secondPointFromWorld.Z())); + ignition::math::Vector3d(endPointFromWorld.X(), + endPointFromWorld.Y(), endPointFromWorld.Z())); } ////////////////////////////////////////////////// @@ -198,15 +252,30 @@ void OpticalTactilePluginVisualization::RequestNormalForcesMarkerMsgs( ignition::msgs::Marker &_positionMarkerMsg, ignition::msgs::Marker &_forceMarkerMsg) { - this->node.Request("/marker", _forceMarkerMsg); this->node.Request("/marker", _positionMarkerMsg); + this->node.Request("/marker", _forceMarkerMsg); // Let the messages be initialized again this->normalForcesMsgsAreInitialized = false; } -} -} -} -} +////////////////////////////////////////////////// +void OpticalTactilePluginVisualization::RemoveNormalForcesAndContactsMarkers() +{ + ignition::msgs::Marker positionMarkerMsg; + ignition::msgs::Marker forceMarkerMsg; + ignition::msgs::Marker contactMarkerMsg; + + positionMarkerMsg.set_ns("positions_" + this->modelName); + positionMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + forceMarkerMsg.set_ns("forces_" + this->modelName); + forceMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + contactMarkerMsg.set_ns("contacts_" + this->modelName); + contactMarkerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); + + this->node.Request("/marker", positionMarkerMsg); + this->node.Request("/marker", forceMarkerMsg); + this->node.Request("/marker", contactMarkerMsg); } diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 5c74b08792..978b3e5aca 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -23,6 +23,9 @@ #include #include +#include + +#include "ignition/gazebo/components/ContactSensorData.hh" namespace ignition { @@ -36,7 +39,12 @@ namespace systems namespace optical_tactile_sensor { // This class handles the configuration and process of visualizing the - // different elements needed for the Optical Tactile Sensor plugin + // different elements needed for the Optical Tactile Sensor plugin. + // Terminology: + // "Contacts" refers to data from the contact sensor based on physics. + // "Normal forces" refers to post-processed data from the depth camera based + // purely on imagery. + // These two sets of data are currently disjoint and visualized separately. class OpticalTactilePluginVisualization { /// \brief Constructor @@ -48,25 +56,44 @@ namespace optical_tactile_sensor /// \param[in] _visualizationResolution Value of the /// visualizationResolution attribute public: OpticalTactilePluginVisualization( - std::string &_modelName, - ignition::math::Vector3d &_sensorSize, - double &_forceLength, - float &_cameraUpdateRate, - ignition::math::Pose3f &_depthCameraOffset, - int &_visualizationResolution); + std::string &_modelName, + ignition::math::Vector3d &_sensorSize, + double &_forceLength, + float &_cameraUpdateRate, + ignition::math::Pose3f &_depthCameraOffset, + int &_visualizationResolution); /// \brief Initialize the marker message representing the optical tactile /// sensor /// \param[out] _sensorMarkerMsg Message for visualizing the sensor private: void InitializeSensorMarkerMsg( - ignition::msgs::Marker &_sensorMarkerMsg); + ignition::msgs::Marker &_sensorMarkerMsg); /// \brief Request the "/marker" service for the sensor marker. /// This can be helpful when debbuging, given that there shouldn't be a /// visual tag in the plugin's model /// \param[in] _sensorPose Pose of the optical tactile sensor public: void RequestSensorMarkerMsg( - ignition::math::Pose3f const &_sensorPose); + ignition::math::Pose3f const &_sensorPose); + + /// \brief Initialize the marker message representing the contacts from the + /// contact sensor + /// \param[out] _contactsMarkerMsg Message for visualizing the contacts + private: void InitializeContactsMarkerMsg( + ignition::msgs::Marker &_contactsMarkerMsg); + + /// \brief Add a contact to the marker message representing the contacts + /// from the contact sensor based on physics + /// \param[in] _contact Contact to be added + /// \param[out] _contactsMarkerMsg Message for visualizing the contacts + public: void AddContactToMarkerMsg( + ignition::msgs::Contact const &_contact, + ignition::msgs::Marker &_contactsMarkerMsg); + + /// \brief Request the "/marker" service for the contacts marker. + /// \param[in] _contacts Contacts to visualize + public: void RequestContactsMarkerMsg( + components::ContactSensorData const *_contacts); /// \brief Initialize the marker messages representing the normal forces /// \param[out] _positionMarkerMsg Message for visualizing the contact @@ -74,11 +101,11 @@ namespace optical_tactile_sensor /// \param[out] _forceMarkerMsg Message for visualizing the contact /// normal forces private: void InitializeNormalForcesMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); - /// \brief Add a normal force to the marker messages representing the - /// normal forces computed + /// \brief Create a marker messages representing the normal force computed + /// from depth camera /// \param[out] _positionMarkerMsg Message for visualizing the contact /// positions /// \param[out] _forceMarkerMsg Message for visualizing the contact @@ -90,11 +117,11 @@ namespace optical_tactile_sensor /// \param[in] _sensorWorldPose Pose of the plugin's model referenced to /// the world's origin public: void AddNormalForceToMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg, - ignition::math::Vector3f &_position, - ignition::math::Vector3f &_normalForce, - ignition::math::Pose3f &_sensorWorldPose); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg, + ignition::math::Vector3f &_position, + ignition::math::Vector3f &_normalForce, + ignition::math::Pose3f &_sensorWorldPose); /// \brief Request the "/marker" service for the marker messages /// representing the normal forces @@ -103,8 +130,11 @@ namespace optical_tactile_sensor /// \param[in] _forceMarkerMsg Message for visualizing the contact /// normal forces public: void RequestNormalForcesMarkerMsgs( - ignition::msgs::Marker &_positionMarkerMsg, - ignition::msgs::Marker &_forceMarkerMsg); + ignition::msgs::Marker &_positionMarkerMsg, + ignition::msgs::Marker &_forceMarkerMsg); + + /// \brief Remove all normal forces and contact markers + public: void RemoveNormalForcesAndContactsMarkers(); /// \brief Transport node to request the /marker service private: ignition::transport::Node node; @@ -130,10 +160,10 @@ namespace optical_tactile_sensor /// \brief Whether the normal forces messages are initialized or not private: bool normalForcesMsgsAreInitialized{false}; }; -} -} -} -} -} +} // namespace optical_tactile_sensor +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc index 0298e71530..b8374f3a55 100644 --- a/src/systems/particle_emitter2/ParticleEmitter2.cc +++ b/src/systems/particle_emitter2/ParticleEmitter2.cc @@ -242,7 +242,6 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, components::ParticleEmitterCmd::typeId, ComponentState::OneTimeChange); } - } this->dataPtr->userCmd.clear(); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c290b547f3..b496aaea27 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -111,6 +112,7 @@ #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" #include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" @@ -119,6 +121,7 @@ #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/HaltMotion.hh" #include "CanonicalLinkModelTracker.hh" #include "EntityFeatureMap.hh" @@ -290,6 +293,42 @@ class ignition::gazebo::systems::PhysicsPrivate return _a == _b; }}; + /// \brief msgs::Contacts equality comparison function. + public: std::function + contactsEql { [](const msgs::Contacts &_a, + const msgs::Contacts &_b) + { + if (_a.contact_size() != _b.contact_size()) + { + return false; + } + + for (int i = 0; i < _a.contact_size(); ++i) + { + if (_a.contact(i).position_size() != + _b.contact(i).position_size()) + { + return false; + } + + for (int j = 0; j < _a.contact(i).position_size(); + ++j) + { + auto pos1 = _a.contact(i).position(j); + auto pos2 = _b.contact(i).position(j); + + if (!math::equal(pos1.x(), pos2.x(), 1e-6) || + !math::equal(pos1.y(), pos2.y(), 1e-6) || + !math::equal(pos1.z(), pos2.z(), 1e-6)) + { + return false; + } + } + } + return true; + }}; + /// \brief Environment variable which holds paths to look for engine plugins public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH"; @@ -395,6 +434,18 @@ class ignition::gazebo::systems::PhysicsPrivate CollisionFeatureList, physics::mesh::AttachMeshShapeFeature>{}; + ////////////////////////////////////////////////// + // Collision detector + /// \brief Feature list for setting and getting the collision detector + public: struct CollisionDetectorFeatureList : ignition::physics::FeatureList< + ignition::physics::CollisionDetector>{}; + + ////////////////////////////////////////////////// + // Solver + /// \brief Feature list for setting and getting the solver + public: struct SolverFeatureList : ignition::physics::FeatureList< + ignition::physics::Solver>{}; + ////////////////////////////////////////////////// // Nested Models @@ -410,7 +461,9 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, CollisionFeatureList, ContactFeatureList, - NestedModelFeatureList>; + NestedModelFeatureList, + CollisionDetectorFeatureList, + SolverFeatureList>; /// \brief A map between world entity ids in the ECM to World Entities in /// ign-physics. @@ -657,6 +710,58 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) auto worldPtrPhys = this->engine->ConstructWorld(world); this->entityWorldMap.AddEntity(_entity, worldPtrPhys); + // Optional world features + auto collisionDetectorComp = + _ecm.Component(_entity); + if (collisionDetectorComp) + { + auto collisionDetectorFeature = + this->entityWorldMap.EntityCast( + _entity); + if (!collisionDetectorFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[CollisionDetectorFeature]. Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + collisionDetectorFeature->SetCollisionDetector( + collisionDetectorComp->Data()); + } + } + + auto solverComp = + _ecm.Component(_entity); + if (solverComp) + { + auto solverFeature = + this->entityWorldMap.EntityCast( + _entity); + if (!solverFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set physics options, but the " + << "phyiscs engine doesn't support feature " + << "[SolverFeature]. Options will be ignored." + << std::endl; + informed = true; + } + } + else + { + solverFeature->SetSolver(solverComp->Data()); + } + } + return true; }); @@ -1268,13 +1373,31 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (nullptr == jointPhys) return true; - // Model is out of battery - if (this->entityOffMap[_ecm.ParentEntity(_entity)]) + auto jointVelFeature = + this->entityJointMap.EntityCast( + _entity); + + auto haltMotionComp = _ecm.Component( + _ecm.ParentEntity(_entity)); + bool haltMotion = false; + if (haltMotionComp) + { + haltMotion = haltMotionComp->Data(); + } + + // Model is out of battery or halt motion has been triggered. + if (this->entityOffMap[_ecm.ParentEntity(_entity)] || haltMotion) { std::size_t nDofs = jointPhys->GetDegreesOfFreedom(); for (std::size_t i = 0; i < nDofs; ++i) { jointPhys->SetForce(i, 0); + + // Halt motion requires the vehicle to come to a full stop, + // while running out of battery can leave existing joint velocity + // in place. + if (haltMotion && jointVelFeature) + jointVelFeature->SetVelocityCommand(i, 0); } return true; } @@ -1379,9 +1502,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) << velocityCmd.size() << ".\n"; } - auto jointVelFeature = - this->entityJointMap.EntityCast( - _entity); if (!jointVelFeature) { return true; @@ -1927,6 +2047,42 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, { IGN_PROFILE("PhysicsPrivate::UpdateSim"); + // Populate world components with default values + _ecm.EachNew( + [&](const Entity &_entity, + const components::World *)->bool + { + // If not provided by ECM, create component with values from physics if + // those features are available + auto collisionDetectorComp = + _ecm.Component(_entity); + if (!collisionDetectorComp) + { + auto collisionDetectorFeature = + this->entityWorldMap.EntityCast( + _entity); + if (collisionDetectorFeature) + { + _ecm.CreateComponent(_entity, components::PhysicsCollisionDetector( + collisionDetectorFeature->GetCollisionDetector())); + } + } + + auto solverComp = _ecm.Component(_entity); + if (!solverComp) + { + auto solverFeature = + this->entityWorldMap.EntityCast(_entity); + if (solverFeature) + { + _ecm.CreateComponent(_entity, + components::PhysicsSolver(solverFeature->GetSolver())); + } + } + + return true; + }); + IGN_PROFILE_BEGIN("Models"); // make sure we have an up-to-date mapping of canonical links to their models @@ -2475,17 +2631,21 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) [&](const Entity &_collEntity1, components::Collision *, components::ContactSensorData *_contacts) -> bool { + msgs::Contacts contactsComp; if (entityContactMap.find(_collEntity1) == entityContactMap.end()) { // Clear the last contact data - *_contacts = components::ContactSensorData(); + auto state = _contacts->SetData(contactsComp, + this->contactsEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + _ecm.SetChanged( + _collEntity1, components::ContactSensorData::typeId, state); return true; } const auto &contactMap = entityContactMap[_collEntity1]; - msgs::Contacts contactsComp; - for (const auto &[collEntity2, contactData] : contactMap) { msgs::Contact *contactMsg = contactsComp.add_contact(); @@ -2499,7 +2659,13 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) position->set_z(contact->point.z()); } } - *_contacts = components::ContactSensorData(contactsComp); + + auto state = _contacts->SetData(contactsComp, + this->contactsEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + _ecm.SetChanged( + _collEntity1, components::ContactSensorData::typeId, state); return true; }); diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index d1c5236013..2877bef288 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 808deca463..d817d55861 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -481,6 +481,7 @@ void TriggeredPublisher::Configure(const Entity &, EntityComponentManager &, EventManager &) { + using namespace std::chrono_literals; sdf::ElementPtr sdfClone = _sdf->Clone(); if (sdfClone->HasElement("input")) { @@ -533,6 +534,14 @@ void TriggeredPublisher::Configure(const Entity &, return; } + // Read trigger delay, if present + if (sdfClone->HasElement("delay_ms")) + { + int ms = sdfClone->Get("delay_ms"); + if (ms > 0) + this->delay = std::chrono::milliseconds(ms); + } + if (sdfClone->HasElement("output")) { for (auto outputElem = sdfClone->GetElement("output"); outputElem; @@ -589,10 +598,20 @@ void TriggeredPublisher::Configure(const Entity &, if (this->MatchInput(_msg)) { { - std::lock_guard lock(this->publishCountMutex); - ++this->publishCount; + if (this->delay > 0ms) + { + std::lock_guard lock(this->publishQueueMutex); + this->publishQueue.push_back(this->delay); + } + else + { + { + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; + } + this->newMatchSignal.notify_one(); + } } - this->newMatchSignal.notify_one(); } }); if (!this->node.Subscribe(this->inputTopic, msgCb)) @@ -648,6 +667,45 @@ void TriggeredPublisher::DoWork() } } +////////////////////////////////////////////////// +void TriggeredPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + using namespace std::chrono_literals; + IGN_PROFILE("TriggeredPublisher::PreUpdate"); + + bool notify = false; + { + std::lock_guard lock1(this->publishQueueMutex); + std::lock_guard lock2(this->publishCountMutex); + // Iterate through the publish queue, and publish messages. + for (auto iter = std::begin(this->publishQueue); + iter != std::end(this->publishQueue);) + { + // Reduce the delay time left for this item in the queue. + *iter -= _info.dt; + + // Publish the message if time is less than or equal to zero + // milliseconds + if (*iter <= 0ms) + { + notify = true; + ++this->publishCount; + + // Remove this publication + iter = this->publishQueue.erase(iter); + } + else + { + ++iter; + } + } + } + + if (notify) + this->newMatchSignal.notify_one(); +} + ////////////////////////////////////////////////// bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) { @@ -667,7 +725,8 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) IGNITION_ADD_PLUGIN(TriggeredPublisher, ignition::gazebo::System, - TriggeredPublisher::ISystemConfigure) + TriggeredPublisher::ISystemConfigure, + TriggeredPublisher::ISystemPreUpdate) IGNITION_ADD_PLUGIN_ALIAS(TriggeredPublisher, "ignition::gazebo::systems::TriggeredPublisher") diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 1fb7d61050..a20aeb1eea 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -37,7 +37,8 @@ namespace systems /// \brief The triggered publisher system publishes a user specified message /// on an output topic in response to an input message that matches user - /// specified criteria. + /// specified criteria. An optional simulation time delay can be used + /// delay message publication. /// /// ## System Parameters /// @@ -73,6 +74,9 @@ namespace systems /// the human-readable representation of a protobuf message as used by /// `ign topic` for publishing messages /// + /// ``: Integer number of milliseconds, in simulation time, to + /// delay publication. + /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} @@ -153,9 +157,9 @@ namespace systems /// The current implementation of this system does not support specifying a /// subfield of a repeated field in the "field" attribute. i.e, if /// `field="f1.f2"`, `f1` cannot be a repeated field. - class TriggeredPublisher : - public System, - public ISystemConfigure + class TriggeredPublisher : public System, + public ISystemConfigure, + public ISystemPreUpdate { /// \brief Constructor public: TriggeredPublisher() = default; @@ -169,6 +173,11 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) override; + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + /// \brief Thread that handles publishing output messages public: void DoWork(); @@ -224,6 +233,15 @@ namespace systems /// \brief Flag for when the system is done and the worker thread should /// stop private: std::atomic done{false}; + + /// \brief Publish delay time. This is in simulation time. + private: std::chrono::steady_clock::duration delay{0}; + + /// \brief Queue of publication times. + private: std::vector publishQueue; + + /// \brief Mutex to synchronize access to publishQueue + private: std::mutex publishQueueMutex; }; } } diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 3a299a5ba2..c395c4c6ff 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -53,6 +53,9 @@ #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/Sensor.hh" using namespace ignition; using namespace gazebo; @@ -78,6 +81,13 @@ class UserCommandsInterface /// \brief World entity. public: Entity worldEntity{kNullEntity}; + + /// \brief Check if there's a contact sensor connected to a collision + /// component + /// \param[in] _collision Collision entity to be checked + /// \return True if a contact sensor is connected to the collision entity, + /// false otherwise + public: bool HasContactSensor(const Entity _collision); }; /// \brief All user commands should inherit from this class so they can be @@ -235,6 +245,32 @@ class PhysicsCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; }; + +/// \brief Command to enable a collision component. +class EnableCollisionCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message identifying the collision to be enabled. + /// \param[in] _iface Pointer to user commands interface. + public: EnableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; + +/// \brief Command to disable a collision component. +class DisableCollisionCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message identifying the collision to be disabled. + /// \param[in] _iface Pointer to user commands interface. + public: DisableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; } } } @@ -288,6 +324,22 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PhysicsService(const msgs::Physics &_req, msgs::Boolean &_res); + /// \brief Callback for enable collision service + /// \param[in] _req Request containing collision entity. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the collision will be successfully enabled. + /// \return True if successful. + public: bool EnableCollisionService( + const msgs::Entity &_req, msgs::Boolean &_res); + + /// \brief Callback for disable collision service + /// \param[in] _req Request containing collision entity. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the collision will be successfully disabled. + /// \return True if successful. + public: bool DisableCollisionService( + const msgs::Entity &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -310,6 +362,38 @@ UserCommands::UserCommands() : System(), ////////////////////////////////////////////////// UserCommands::~UserCommands() = default; +////////////////////////////////////////////////// +bool UserCommandsInterface::HasContactSensor(const Entity _collision) +{ + auto *linkEntity = ecm->Component(_collision); + auto allLinkSensors = + ecm->EntitiesByComponents(components::Sensor(), + components::ParentEntity(*linkEntity)); + + for (auto const &sensor : allLinkSensors) + { + // Check if it is a contact sensor + auto isContactSensor = + ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); + if (!isContactSensor) + continue; + + // Check if sensor is connected to _collision + auto componentName = ecm->Component(_collision); + std::string collisionName = componentName->Data(); + auto sensorSDF = ecm->Component(sensor)->Data(); + auto sensorCollisionName = + sensorSDF->GetElement("contact")->Get("collision"); + + if (collisionName == sensorCollisionName) + { + return true; + } + } + + return false; +} + ////////////////////////////////////////////////// void UserCommands::Configure(const Entity &_entity, const std::shared_ptr &, @@ -376,6 +460,24 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::PhysicsService, this->dataPtr.get()); ignmsg << "Physics service on [" << physicsService << "]" << std::endl; + + // Enable collision service + std::string enableCollisionService{ + "/world/" + validWorldName + "/enable_collision"}; + this->dataPtr->node.Advertise(enableCollisionService, + &UserCommandsPrivate::EnableCollisionService, this->dataPtr.get()); + + ignmsg << "Enable collision service on [" << enableCollisionService << "]" + << std::endl; + + // Disable collision service + std::string disableCollisionService{ + "/world/" + validWorldName + "/disable_collision"}; + this->dataPtr->node.Advertise(disableCollisionService, + &UserCommandsPrivate::DisableCollisionService, this->dataPtr.get()); + + ignmsg << "Disable collision service on [" << disableCollisionService << "]" + << std::endl; } ////////////////////////////////////////////////// @@ -506,6 +608,44 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::EnableCollisionService(const msgs::Entity &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +bool UserCommandsPrivate::DisableCollisionService(const msgs::Entity &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req, msgs::Boolean &_res) @@ -812,7 +952,6 @@ bool RemoveCommand::Execute() return true; } - ////////////////////////////////////////////////// LightCommand::LightCommand(msgs::Light *_msg, std::shared_ptr &_iface) @@ -991,6 +1130,108 @@ bool PhysicsCommand::Execute() return true; } +////////////////////////////////////////////////// +EnableCollisionCommand::EnableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool EnableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->iface->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Create ContactSensorData component + auto contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (contactDataComp) + { + ignwarn << "Can't create component that already exists" << std::endl; + return false; + } + + this->iface->ecm-> + CreateComponent(entityMsg->id(), components::ContactSensorData()); + igndbg << "Enabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + +////////////////////////////////////////////////// +DisableCollisionCommand::DisableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool DisableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->iface->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Remove ContactSensorData component + auto *contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (!contactDataComp) + { + ignwarn << "No ContactSensorData detected inside entity " << entityMsg->id() + << std::endl; + return false; + } + + this->iface->ecm-> + RemoveComponent(entityMsg->id(), components::ContactSensorData::typeId); + + igndbg << "Disabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 76a70ea0b3..d1a2e310dd 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -152,10 +152,6 @@ class ignition::gazebo::systems::WindEffectsPrivate /// valid. public: bool validConfig{false}; - /// \brief Whether links have been initialized. Initialization involves - /// creating components this system needs on links that are affected by wind. - public: bool linksInitialized{false}; - /// \brief Mutex to protect currWindVelSeed and windEnabled public: std::mutex windInfoMutex; @@ -550,45 +546,39 @@ void WindEffects::PreUpdate(const UpdateInfo &_info, // Process commands this->dataPtr->ProcessCommandQueue(_ecm); - if (this->dataPtr->validConfig) + if (!this->dataPtr->validConfig) + return; + + _ecm.EachNew( + [&](const Entity &_entity, components::Link *, + components::WindMode *_windMode) -> bool { - if (!this->dataPtr->linksInitialized) + if (_windMode->Data()) { - _ecm.Each( - [&](const Entity &_entity, components::Link *, - components::WindMode *_windMode) -> bool - { - if (_windMode->Data()) - { - // Create a WorldLinearVelocity component on the link so that - // physics can populate it - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, components::WorldPose()); - } - } - return true; - }); - - this->dataPtr->linksInitialized = true; + // Create a WorldLinearVelocity component on the link so that + // physics can populate it + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + components::WorldLinearVelocity()); + } + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, components::WorldPose()); + } } - else - { - if (_info.paused) - return; + return true; + }); - if (!this->dataPtr->currentWindInfo.enable_wind()) - return; + if (_info.paused) + return; + + if (!this->dataPtr->currentWindInfo.enable_wind()) + return; + + this->dataPtr->UpdateWindVelocity(_info, _ecm); + this->dataPtr->ApplyWindForce(_info, _ecm); - this->dataPtr->UpdateWindVelocity(_info, _ecm); - this->dataPtr->ApplyWindForce(_info, _ecm); - } - } } IGNITION_ADD_PLUGIN(WindEffects, System, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8e28185c14..11f7070a44 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -19,6 +19,7 @@ set(tests examples_build.cc follow_actor_system.cc fuel_cached_server.cc + halt_motion.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc @@ -64,6 +65,7 @@ set(tests_needing_display camera_video_record_system.cc depth_camera.cc gpu_lidar.cc + optical_tactile_plugin.cc rgbd_camera.cc sensors_system.cc thermal_system.cc diff --git a/test/integration/components.cc b/test/integration/components.cc index 41b10130dc..26962f5190 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -945,31 +945,33 @@ TEST_F(ComponentsTest, LogicalAudioSourcePlayInfo) TEST_F(ComponentsTest, LogicalMicrophone) { logical_audio::Microphone mic1; - mic1.id = 0; + mic1.id = 4; mic1.volumeDetectionThreshold = 0.5; logical_audio::Microphone mic2; - mic2.id = 1; - mic2.volumeDetectionThreshold = mic1.volumeDetectionThreshold; + mic2.id = 8; + mic2.volumeDetectionThreshold = 0.8; // create components auto comp1 = components::LogicalMicrophone(mic1); auto comp2 = components::LogicalMicrophone(mic2); // equality operators - EXPECT_NE(mic1, mic2); - EXPECT_FALSE(mic1 == mic2); - EXPECT_TRUE(mic1 != mic2); + EXPECT_NE(comp1, comp2); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); // stream operators std::ostringstream ostr; comp1.Serialize(ostr); - EXPECT_EQ("0 0.5", ostr.str()); + EXPECT_EQ("4 0.5", ostr.str()); std::istringstream istr(ostr.str()); components::LogicalMicrophone comp3; comp3.Deserialize(istr); EXPECT_EQ(comp1, comp3); + EXPECT_DOUBLE_EQ(comp1.Data().volumeDetectionThreshold, + comp3.Data().volumeDetectionThreshold); } ///////////////////////////////////////////////// diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 30223d0284..c09e9d74b8 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -54,145 +54,165 @@ class DiffDriveTest : public ::testing::TestWithParam const std::string &_cmdVelTopic, const std::string &_odomTopic) { - // Start server - ServerConfig serverConfig; - serverConfig.SetSdfFile(_sdfFile); - - Server server(serverConfig); - EXPECT_FALSE(server.Running()); - EXPECT_FALSE(*server.Running(0)); - - // Create a system that records the vehicle poses - test::Relay testSystem; - - std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - auto id = _ecm.EntityByComponents( - components::Model(), - components::Name("vehicle")); - EXPECT_NE(kNullEntity, id); + /// \param[in] forward If forward is true, the 'max_acceleration' + // and 'max_velocity' properties are tested, as the movement + // is forward, otherwise 'min_acceleration' and 'min_velocity' + // properties are tested. + auto testCmdVel = [&](bool forward){ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); - auto poseComp = _ecm.Component(id); - ASSERT_NE(nullptr, poseComp); + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); - poses.push_back(poseComp->Data()); - }); - server.AddSystem(testSystem.systemPtr); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); - // Run server and check that vehicle didn't move - server.Run(true, 1000, false); + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); - EXPECT_EQ(1000u, poses.size()); + EXPECT_EQ(1000u, poses.size()); - for (const auto &pose : poses) - { - EXPECT_EQ(poses[0], pose); - } - - // Get odometry messages - double period{1.0 / 50.0}; - double lastMsgTime{1.0}; - std::vector odomPoses; - std::function odomCb = - [&](const msgs::Odometry &_msg) + for (const auto &pose : poses) { - ASSERT_TRUE(_msg.has_header()); - ASSERT_TRUE(_msg.header().has_stamp()); - - double msgTime = - static_cast(_msg.header().stamp().sec()) + - static_cast(_msg.header().stamp().nsec()) * 1e-9; - - EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); - lastMsgTime = msgTime; - - odomPoses.push_back(msgs::Convert(_msg.pose())); - }; - - // Publish command and check that vehicle moved - transport::Node node; - auto pub = node.Advertise(_cmdVelTopic); - node.Subscribe(_odomTopic, odomCb); - - msgs::Twist msg; - - // Avoid wheel slip by limiting acceleration - // Avoid wheel slip by limiting acceleration (1 m/s^2) - // and max velocity (0.5 m/s). - // See parameters - // in "/test/worlds/diff_drive.sdf". - test::Relay velocityRamp; - const double desiredLinVel = 10.5; - const double desiredAngVel = 0.2; - velocityRamp.OnPreUpdate( - [&](const gazebo::UpdateInfo &/*_info*/, - const gazebo::EntityComponentManager &) + EXPECT_EQ(poses[0], pose); + } + + // Get odometry messages + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) { - msgs::Set(msg.mutable_linear(), - math::Vector3d(desiredLinVel, 0, 0)); - msgs::Set(msg.mutable_angular(), - math::Vector3d(0.0, 0, desiredAngVel)); - pub.Publish(msg); - }); - - server.AddSystem(velocityRamp.systemPtr); - - server.Run(true, 3000, false); + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + + // Avoid wheel slip by limiting acceleration (1 m/s^2) + // and max velocity (0.5 m/s). + // See and parameters + // in "/test/worlds/diff_drive.sdf". + // See , , and + // parameters in "/test/worlds/diff_drive.sdf". + test::Relay velocityRamp; + const int movementDirection = (forward ? 1 : -1); + double desiredLinVel = movementDirection * 10.5; + double desiredAngVel = 0.2; + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &) + { + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Odometry calculates the pose of a point that is located half way + // between the two wheels, not the origin of the model. For example, + // if the vehicle is commanded to rotate in place, the vehicle will + // rotate about the point half way between the two wheels, thus, + // the odometry position will remain zero. + // However, since the model origin is offset, the model position will + // change. To find the final pose of the model, we have to do the + // following similarity transformation + math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); + auto finalModelFramePose = + tOdomModel * odomPoses.back() * tOdomModel.Inverse(); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + + auto expectedLowerPosition = + (forward ? poses[0].Pos() : poses[3999].Pos()); + auto expectedGreaterPosition = + (forward ? poses[3999].Pos() : poses[0].Pos()); + + EXPECT_LT(expectedLowerPosition.X(), expectedGreaterPosition.X()); + EXPECT_LT(expectedLowerPosition.Y(), expectedGreaterPosition.Y()); + EXPECT_NEAR(expectedLowerPosition.Z(), expectedGreaterPosition.Z(), tol); + EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); + EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); + + // The value from odometry will be close, but not exactly the ground truth + // pose of the robot model. This is partially due to throttling the + // odometry publisher, partially due to a frame difference between the + // odom frame and the model frame, and partially due to wheel slip. + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + + // Verify velocity and acceleration boundaries. + // Moving time. + double t = 3.0; + double d = poses[3999].Pos().Distance(poses[0].Pos()); + double v0 = 0; + double v = d / t; + double a = (v - v0) / t; + + EXPECT_LT(v, 0.5); + EXPECT_LT(a, 1); + EXPECT_GT(v, -0.5); + EXPECT_GT(a, -1); - // Poses for 4s - ASSERT_EQ(4000u, poses.size()); + }; - int sleep = 0; - int maxSleep = 30; - for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - ASSERT_NE(maxSleep, sleep); - - // Odometry calculates the pose of a point that is located half way between - // the two wheels, not the origin of the model. For example, if the - // vehicle is commanded to rotate in place, the vehicle will rotate about - // the point half way between the two wheels, thus, the odometry position - // will remain zero. - // However, since the model origin is offset, the model position will - // change. To find the final pose of the model, we have to do the - // following similarity transformation - math::Pose3d tOdomModel(0.554283, 0, -0.325, 0, 0, 0); - auto finalModelFramePose = - tOdomModel * odomPoses.back() * tOdomModel.Inverse(); - - // Odom for 3s - ASSERT_FALSE(odomPoses.empty()); - EXPECT_EQ(150u, odomPoses.size()); - - EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); - EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); - EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); - EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); - EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); - EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); - - // The value from odometry will be close, but not exactly the ground truth - // pose of the robot model. This is partially due to throttling the - // odometry publisher, partially due to a frame difference between the - // odom frame and the model frame, and partially due to wheel slip. - EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); - EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); - EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); - EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); - - // Max velocities/accelerations expectations. - // Moving time. - double t = 3.0; - double d = poses[3999].Pos().Distance(poses[1000].Pos()); - const double v0 = 0; - double v = d / t; - double a = (v - v0) / t; - EXPECT_LT(v, 0.5); - EXPECT_LT(a, 1); + testCmdVel(true /*test forward movement*/); + testCmdVel(false /*test backward movement*/); } }; diff --git a/test/integration/halt_motion.cc b/test/integration/halt_motion.cc new file mode 100644 index 0000000000..ec3b6c0159 --- /dev/null +++ b/test/integration/halt_motion.cc @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/HaltMotion.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test DiffDrive system +class HaltMotionTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(1); + ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + /// \param[in] _odomTopic Odometry topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + testSystem.OnPreUpdate([&poses](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager & _ecm) + { + auto model = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle"));; + EXPECT_NE(kNullEntity, model); + if (!_ecm.Component(model)) + { + _ecm.CreateComponent(model, components::HaltMotion(false)); + } + if (poses.size() == 4000u && + !_ecm.Component(model)->Data()) + { + _ecm.Component(model)->Data() = true; + } + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + const double desiredLinVel = 10.5; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, 0)); + pub.Publish(msg); + + server.Run(true, 1000, false); + + msgs::Set(msg.mutable_linear(), + math::Vector3d(0, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, 0)); + pub.Publish(msg); + + server.Run(true, 2000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + server.Run(true, 1000, false); + + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, 0)); + pub.Publish(msg); + + server.Run(true, 4000, false); + + EXPECT_EQ(9000u, poses.size()); + + for (uint64_t i = 4001; i < poses.size(); ++i) + { + EXPECT_EQ(poses[3999], poses[i]); + } + } +}; + +///////////////////////////////////////////////// +TEST_P(HaltMotionTest, PublishCmd) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/diff_drive.sdf", + "/model/vehicle/cmd_vel"); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, HaltMotionTest, + ::testing::Range(1, 2)); diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc new file mode 100644 index 0000000000..ea6ec02e85 --- /dev/null +++ b/test/integration/optical_tactile_plugin.cc @@ -0,0 +1,178 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test OpticalTactilePlugin system +class OpticalTactilePluginTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + public: void StartServer(const std::string &_sdfFile) + { + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + server = std::make_unique(serverConfig); + using namespace std::chrono_literals; + server->SetUpdatePeriod(0ns); + + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + } + + public: ignition::math::Vector3f MapPointCloudData( + const uint64_t &_i, + const uint64_t &_j) + { + // Initialize return variable + ignition::math::Vector3f measuredPoint(0, 0, 0); + + std::string data = this->normalForces.data(); + char *msgBuffer = data.data(); + + // Number of bytes from the beginning of the pointer (image coordinates at + // 0,0) to the desired (i,j) position + uint32_t msgBufferIndex = + _j * this->normalForces.step() + _i * 3 * sizeof(float); + + measuredPoint.X() = static_cast( + msgBuffer[msgBufferIndex]); + + measuredPoint.Y() = static_cast( + msgBuffer[msgBufferIndex + sizeof(float)]); + + measuredPoint.Z() = static_cast( + msgBuffer[msgBufferIndex + 2*sizeof(float)]); + + return measuredPoint; + } + + public: msgs::Image normalForces; + + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// The test checks the normal forces on the corners of the box-shaped sensor +// Fails to load Ogre plugin on macOS +TEST_F(OpticalTactilePluginTest, + IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ForcesOnPlane)) +{ + // World with moving entities + const auto sdfPath = common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", "optical_tactile_plugin.sdf"); + this->StartServer(sdfPath); + + bool receivedMsg{false}; + math::Vector3f upperLeftNormalForce(0, 0, 0); + math::Vector3f upperRightNormalForce(0, 0, 0); + math::Vector3f lowerLeftNormalForce(0, 0, 0); + math::Vector3f lowerRightNormalForce(0, 0, 0); + auto normalForcesCb = std::function( + [&](const msgs::Image &_image) + { + this->normalForces = _image; + + upperRightNormalForce = this->MapPointCloudData(1, 1); + upperLeftNormalForce = this->MapPointCloudData((_image.width() - 2), 1); + lowerLeftNormalForce = this->MapPointCloudData(1, (_image.height() - 2)); + lowerRightNormalForce = this->MapPointCloudData( + _image.width() - 2, _image.height() - 2); + receivedMsg = true; + }); + + transport::Node node; + node.Subscribe("/optical_tactile_sensor/normal_forces", normalForcesCb); + + // Check that there are no contacts nor forces yet + EXPECT_EQ(math::Vector3f(0, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + + // Let the depth camera generate data + server->Run(true, 1000, false); + + // Give some time for messages to propagate + int sleep{0}; + int maxSleep{10}; + while (!receivedMsg && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + EXPECT_EQ(maxSleep, sleep); + EXPECT_FALSE(receivedMsg); + receivedMsg = false; + + // Check that there are no forces before the plugin is enabled + EXPECT_EQ(math::Vector3f(0, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(0, 0, 0), lowerRightNormalForce); + + // Enable the plugin + msgs::Boolean req; + req.set_data(true); + bool executed = node.Request("/optical_tactile_sensor/enable", req); + EXPECT_TRUE(executed); + + // Let the plugin generate data again + server->Run(true, 2000, false); + + // Give some time for messages to propagate + sleep = 0; + while (!receivedMsg && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + sleep++; + } + EXPECT_NE(maxSleep, sleep); + EXPECT_TRUE(receivedMsg); + receivedMsg = false; + + // Check the values of the forces + EXPECT_EQ(math::Vector3f(-1, 0, 0), upperRightNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), upperLeftNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), lowerLeftNormalForce); + EXPECT_EQ(math::Vector3f(-1, 0, 0), lowerRightNormalForce); +} diff --git a/test/integration/particle_emitter2.cc b/test/integration/particle_emitter2.cc index 1550c550c6..cb1514276d 100644 --- a/test/integration/particle_emitter2.cc +++ b/test/integration/particle_emitter2.cc @@ -84,7 +84,6 @@ TEST_F(ParticleEmitter2Test, SDFLoad) const components::Name *_name, const components::Pose *_pose) -> bool { - if (_name->Data() == "smoke_emitter") { updateCustomChecked = true; @@ -119,6 +118,24 @@ TEST_F(ParticleEmitter2Test, SDFLoad) // will not be able to find a file that does not exist EXPECT_EQ("/path/to/dummy_image.png", _emitter->Data().color_range_image().data()); + + // particle scatter ratio is temporarily stored in header + bool hasParticleScatterRatio = false; + for (int i = 0; i < _emitter->Data().header().data_size(); ++i) + { + for (int j = 0; + j < _emitter->Data().header().data(i).value_size(); ++j) + { + if (_emitter->Data().header().data(i).key() == + "particle_scatter_ratio") + { + EXPECT_DOUBLE_EQ(0.01, math::parseFloat( + _emitter->Data().header().data(i).value(0))); + hasParticleScatterRatio = true; + } + } + } + EXPECT_TRUE(hasParticleScatterRatio); } else { diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index e1ac1762ec..d1e34607a2 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -53,6 +53,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" @@ -1373,3 +1374,87 @@ TEST_F(PhysicsSystemFixture, NestedModelIndividualCanonicalLinks) EXPECT_DOUBLE_EQ(model11WorldPose.Z(), model10It->second.Z()); EXPECT_DOUBLE_EQ(model12WorldPose.Z(), model10It->second.Z()); } + +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, DefaultPhysicsOptions) +{ + ignition::gazebo::ServerConfig serverConfig; + + bool checked{false}; + + // Create a system to check components + test::Relay testSystem; + testSystem.OnPostUpdate( + [&checked](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::World *, + const components::PhysicsCollisionDetector *_collisionDetector, + const components::PhysicsSolver *_solver)->bool + { + EXPECT_NE(nullptr, _collisionDetector); + if (_collisionDetector) + { + EXPECT_EQ("ode", _collisionDetector->Data()); + } + EXPECT_NE(nullptr, _solver); + if (_solver) + { + EXPECT_EQ("DantzigBoxedLcpSolver", _solver->Data()); + } + checked = true; + return true; + }); + }); + + gazebo::Server server(serverConfig); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + EXPECT_TRUE(checked); +} + +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, PhysicsOptions) +{ + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "physics_options.sdf")); + + bool checked{false}; + + // Create a system to check components + test::Relay testSystem; + testSystem.OnPostUpdate( + [&checked](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::World *, + const components::PhysicsCollisionDetector *_collisionDetector, + const components::PhysicsSolver *_solver)->bool + { + EXPECT_NE(nullptr, _collisionDetector); + if (_collisionDetector) + { + EXPECT_EQ("bullet", _collisionDetector->Data()); + } + EXPECT_NE(nullptr, _solver); + if (_solver) + { + EXPECT_EQ("pgs", _solver->Data()); + } + checked = true; + return true; + }); + }); + + gazebo::Server server(serverConfig); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + EXPECT_TRUE(checked); +} diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index aad2f7dc30..849c409b31 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -224,7 +224,6 @@ TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem)) maxTemp = info.max.Kelvin(); return true; }); - }); server.AddSystem(testSystem.systemPtr); diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 436ad8b998..d3bed25446 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -562,6 +562,41 @@ TEST_F(TriggeredPublisherTest, SubfieldsOfRepeatedFieldsNotSupported) EXPECT_EQ(0u, recvCount); } +TEST_F(TriggeredPublisherTest, TriggerDelay) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_13"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_13", msgCb); + IGN_SLEEP_MS(100ms); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + } + waitUntil(1000, [&]{return pubCount == recvCount;}); + + // Delay has been specified, but simulation is not running. No messages + // should have been received. + EXPECT_EQ(0u, recvCount); + + // The simulation delay is 1000ms, which is equal to 1000 steps. Run + // for 999 steps, and the count should still be zero. Take one additional + // step and all the messages should arrive. + this->server->Run(true, 999, false); + waitUntil(1000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(0u, recvCount); + this->server->Run(true, 1, false); + waitUntil(1000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) { transport::Node node; diff --git a/test/integration/view.cc b/test/integration/view.cc index c45f4be560..b7497e10f3 100644 --- a/test/integration/view.cc +++ b/test/integration/view.cc @@ -26,15 +26,15 @@ #include #include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentStorage.hh" #include "ignition/gazebo/Types.hh" -#include "ignition/gazebo/detail/View.hh" -#include "ignition/gazebo/detail/ComponentStorage.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/detail/View.hh" using namespace ignition; using namespace gazebo; @@ -62,10 +62,10 @@ class ViewTest : public::testing::Test } }; -/// \brief Helper class that wraps ComponentStorage to provide a simplified -/// API for working with entities and their components. This class also stores -/// a set of user-defined views and handles updating views as needed whenever -/// an entity's components are modified. +/// \brief Helper class that wraps EntityComponentStorage to provide a +/// simplified API for working with entities and their components. This class +/// also stores a set of user-defined views and handles updating views as +/// needed whenever an entity's components are modified. class StorageViewWrapper { /// \brief Constructor @@ -125,7 +125,7 @@ class StorageViewWrapper void AddNewComponent(const Entity _entity, const ComponentTypeT &_component, bool _isNewEntity = false) { - EXPECT_EQ(detail::ComponentAdditionResult::NEW_ADDITION, + EXPECT_EQ(ComponentAdditionResult::NEW_ADDITION, this->AddComponent(_entity, _component)); for (auto &view : this->views) @@ -156,7 +156,7 @@ class StorageViewWrapper void ReAddComponent(const Entity _entity, bool _kNewEntity, const ComponentTypeT &_component) { - EXPECT_EQ(detail::ComponentAdditionResult::RE_ADDITION, + EXPECT_EQ(ComponentAdditionResult::RE_ADDITION, this->AddComponent(_entity, _component)); // after the component has been "re-added", the actual data of the component @@ -179,7 +179,7 @@ class StorageViewWrapper void ModifyComponent(const Entity _entity, const ComponentTypeT &_component) { - EXPECT_EQ(detail::ComponentAdditionResult::MODIFICATION, + EXPECT_EQ(ComponentAdditionResult::MODIFICATION, this->AddComponent(_entity, _component)); auto entityComp = this->Component(_entity); ASSERT_NE(nullptr, entityComp); @@ -187,8 +187,8 @@ class StorageViewWrapper EXPECT_EQ(entityComp->Data(), _component.Data()); } - /// \brief Helper function that uses ComponentStorage::ValidComponent to get - /// a component of a particular type that belongs to an entity. + /// \brief Helper function that uses EntityComponentStorage::ValidComponent + /// to get a component of a particular type that belongs to an entity. /// \param[in] _entity The entity /// \return A pointer to the component of the templated type. If no such /// component exists, nullptr is returned @@ -222,7 +222,7 @@ class StorageViewWrapper /// \param[in] _component The component, which belongs to _entity /// \return The result of the component addition private: template - detail::ComponentAdditionResult AddComponent(const Entity _entity, + ComponentAdditionResult AddComponent(const Entity _entity, const ComponentTypeT &_component) { auto compPtr = components::Factory::Instance()->New( @@ -230,8 +230,8 @@ class StorageViewWrapper return this->storage.AddComponent(_entity, std::move(compPtr)); } - /// \brief The actual detail::ComponentStorage instance - private: detail::ComponentStorage storage; + /// \brief The actual EntityComponentStorage instance + private: EntityComponentStorage storage; /// \brief All of the views that will be updated as needed based on the /// changes being made to this->storage diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index d0bd971669..cfe90648d9 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -326,3 +326,55 @@ TEST_F(WindEffectsTest , TopicsAndServices) lastVelMagnitude = velMagnitude; } } + +/// Test if adding a link with wind after first iteration adds +/// WorldLinearVelocity component properly +TEST_F(WindEffectsTest, WindEntityAddedAfterStart) +{ + const std::string windBox = R"EOF( + + + + 5 5 5 0 0 0 + true + + + + + 1 1 1 + + + + + + )EOF"; + + this->StartServer("/test/worlds/wind_effects.sdf"); + + LinkComponentRecorder + linkVelocityComponent("test_link_wind"); + this->server->AddSystem(linkVelocityComponent.systemPtr); + EXPECT_TRUE(linkVelocityComponent.values.empty()); + + // Run the logger for a time, check it is still empty + this->server->Run(true, 10, false); + EXPECT_TRUE(linkVelocityComponent.values.empty()); + + // Add the box to be logged via the command system + // and check that is not empty + transport::Node node; + msgs::EntityFactory req; + unsigned int timeout = 5000; + std::string service{"/world/wind_demo/create"}; + msgs::Boolean res; + bool result; + + req.set_sdf(windBox); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Now box_wind WorldLinearVelocity component should be added + this->server->Run(true, 10, false); + ASSERT_FALSE(linkVelocityComponent.values.empty()); +} diff --git a/test/worlds/diff_drive.sdf b/test/worlds/diff_drive.sdf index b2b00bb0c4..dcb6a1fc2e 100644 --- a/test/worlds/diff_drive.sdf +++ b/test/worlds/diff_drive.sdf @@ -228,7 +228,9 @@ 1.25 0.3 1 + -1 0.5 + -0.5 diff --git a/test/worlds/diff_drive_custom_topics.sdf b/test/worlds/diff_drive_custom_topics.sdf index 075c1c6744..15a414d1e3 100644 --- a/test/worlds/diff_drive_custom_topics.sdf +++ b/test/worlds/diff_drive_custom_topics.sdf @@ -231,6 +231,8 @@ /model/bar/odom 1 0.5 + -1 + -0.5 diff --git a/test/worlds/models/mesh_with_submeshes/model.sdf b/test/worlds/models/mesh_with_submeshes/model.sdf index 973c4a7bf3..5e6f942368 100644 --- a/test/worlds/models/mesh_with_submeshes/model.sdf +++ b/test/worlds/models/mesh_with_submeshes/model.sdf @@ -31,4 +31,3 @@ - diff --git a/test/worlds/optical_tactile_plugin.sdf b/test/worlds/optical_tactile_plugin.sdf new file mode 100644 index 0000000000..8c8a5770b5 --- /dev/null +++ b/test/worlds/optical_tactile_plugin.sdf @@ -0,0 +1,92 @@ + + + + + + + + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.0030 0 1.57 -1.57 + + + + + 0.005 0.02 0.02 + + + + + 1 + depth_camera + -0.05 0 0 0 0 0 + + + 640 + 480 + R_FLOAT32 + + + 0.030 + 10.0 + + + + + + collision + + + + false + + false + optical_tactile_sensor + false + false + false + 1 + 0.01 + 0.05 + + + + + diff --git a/test/worlds/particle_emitter2.sdf b/test/worlds/particle_emitter2.sdf index 7a8781512b..0fcf08df97 100644 --- a/test/worlds/particle_emitter2.sdf +++ b/test/worlds/particle_emitter2.sdf @@ -121,6 +121,7 @@ 0 1 0 10 /path/to/dummy_image.png + 0.01 diff --git a/test/worlds/physics_options.sdf b/test/worlds/physics_options.sdf new file mode 100644 index 0000000000..d4fcae60e1 --- /dev/null +++ b/test/worlds/physics_options.sdf @@ -0,0 +1,17 @@ + + + + + + bullet + + pgs + + + + + + + diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index e2fbe4dcef..2c9aec273b 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -144,6 +144,12 @@ + + + + 1000 + + diff --git a/test/worlds/wind_effects.sdf b/test/worlds/wind_effects.sdf index 290b80ca5e..af2cfcb2b3 100644 --- a/test/worlds/wind_effects.sdf +++ b/test/worlds/wind_effects.sdf @@ -7,6 +7,10 @@ filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"> + + - - - - -"""%d) - -if __name__ == '__main__': - check_main() - - diff --git a/tools/code_check.sh b/tools/code_check.sh deleted file mode 100755 index fb11060d00..0000000000 --- a/tools/code_check.sh +++ /dev/null @@ -1,129 +0,0 @@ -#!/bin/sh - -# Jenkins will pass -xml, in which case we want to generate XML output -xmlout=0 -if test "$1" = "-xmldir" -a -n "$2"; then - xmlout=1 - xmldir=$2 - mkdir -p $xmldir - rm -rf $xmldir/*.xml - # Assuming that Jenkins called, the `build` directory is sibling to src dir - builddir=../build -else - # This is a heuristic guess; not everyone puts the `build` dir in the src dir - builddir=./build -fi - -# Identify cppcheck version -CPPCHECK_VERSION=`cppcheck --version | sed -e 's@Cppcheck @@'` -CPPCHECK_LT_157=`echo "$CPPCHECK_VERSION 1.57" | \ - awk '{if ($1 < $2) print 1; else print 0}'` - -QUICK_CHECK=0 -if test "$1" = "--quick" -then - QUICK_CHECK=1 - QUICK_SOURCE=$2 - hg_root=`hg root` - if [ "$?" -ne "0" ] ; then - echo This is not an hg repository - exit - fi - cd $hg_root - hg log -r $QUICK_SOURCE > /dev/null - if [ "$?" -ne "0" ] ; then - echo $QUICK_SOURCE is not a valid changeset hash - exit - fi - CHECK_FILES="" - while read line; do - for f in $line; do - CHECK_FILES="$CHECK_FILES `echo $f | grep '\.[ch][ch]*$' | grep -v '^deps'`" - done - done - CPPCHECK_FILES="$CHECK_FILES" - CPPLINT_FILES="$CHECK_FILES" - QUICK_TMP=`mktemp -t asdfXXXXXXXXXX` -else - CHECK_DIRS="./src ./include ./test/integration ./test/regression ./test/performance" - if [ $CPPCHECK_LT_157 -eq 1 ]; then - # cppcheck is older than 1.57, so don't check header files (issue #907) - CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc"` - else - CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" -o -name "*.hh"` - fi - CPPLINT_FILES=`\ - find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h"` -fi - -#cppcheck -CPPCHECK_BASE="cppcheck -q --inline-suppr" -if [ $CPPCHECK_LT_157 -eq 0 ]; then - # use --language argument if 1.57 or greater (issue #907) - CPPCHECK_BASE="$CPPCHECK_BASE --language=c++" -fi -CPPCHECK_INCLUDES="-I ./include -I $builddir -I test -I ./include/ignition/gazebo" -CPPCHECK_RULES="-UM_PI"\ -" --rule-file=./tools/cppcheck_rules/header_guard.rule"\ -" --rule-file=./tools/cppcheck_rules/namespace_AZ.rule" -CPPCHECK_CMD1A="-j 4 --enable=style,performance,portability,information" -CPPCHECK_CMD1B="$CPPCHECK_RULES $CPPCHECK_FILES" -CPPCHECK_CMD1="$CPPCHECK_CMD1A $CPPCHECK_CMD1B" -CPPCHECK_CMD2="--enable=unusedFunction $CPPCHECK_FILES" - -# Checking for missing includes is very time consuming. This is disabled -# for now -CPPCHECK_CMD3="-j 4 --enable=missingInclude $CPPCHECK_FILES $CPPCHECK_INCLUDES" -# CPPCHECK_CMD3="" - -if [ $xmlout -eq 1 ]; then - # Performance, style, portability, and information - ($CPPCHECK_BASE --xml --xml-version=2 $CPPCHECK_CMD1) 2> $xmldir/cppcheck.xml - - # Check the configuration - ($CPPCHECK_BASE --xml --xml-version=2 $CPPCHECK_CMD3) 2> $xmldir/cppcheck-configuration.xml -elif [ $QUICK_CHECK -eq 1 ]; then - for f in $CHECK_FILES; do - prefix=`basename $f | sed -e 's@\..*$@@'` - ext=`echo $f | sed -e 's@^.*\.@@'` - tmp2="$QUICK_TMP"."$ext" - tmp2base=`basename "$QUICK_TMP"` - hg cat -r $QUICK_SOURCE $hg_root/$f > $tmp2 - - # Skip cppcheck for header files if cppcheck is old - DO_CPPCHECK=0 - if [ $ext = 'cc' ]; then - DO_CPPCHECK=1 - elif [ $CPPCHECK_LT_157 -eq 0 ]; then - DO_CPPCHECK=1 - fi - - if [ $DO_CPPCHECK -eq 1 ]; then - $CPPCHECK_BASE $CPPCHECK_CMD1A $CPPCHECK_RULES $tmp2 2>&1 \ - | sed -e "s@$tmp2@$f@g" \ - | grep -v 'use --check-config for details' \ - | grep -v 'Include file: .*not found' - fi - - python $hg_root/tools/cpplint.py --quiet $tmp2 2>&1 \ - | sed -e "s@$tmp2@$f@g" -e "s@$tmp2base@$prefix@g" \ - | grep -v 'Total errors found: 0' - - rm $tmp2 - done - rm $QUICK_TMP -else - # Performance, style, portability, and information - $CPPCHECK_BASE $CPPCHECK_INCLUDES $CPPCHECK_CMD1 2>&1 - - # Check the configuration - $CPPCHECK_BASE $CPPCHECK_CMD3 2>&1 -fi - -# cpplint -if [ $xmlout -eq 1 ]; then - (echo $CPPLINT_FILES | xargs python tools/cpplint.py --extensions=cc,hh --quiet 2>&1) \ - | python tools/cpplint_to_cppcheckxml.py 2> $xmldir/cpplint.xml -elif [ $QUICK_CHECK -eq 0 ]; then - echo $CPPLINT_FILES | xargs python tools/cpplint.py --extensions=cc,hh --quiet 2>&1 -fi diff --git a/tools/cppcheck_rules/header_guard.rule b/tools/cppcheck_rules/header_guard.rule deleted file mode 100644 index 66079516ba..0000000000 --- a/tools/cppcheck_rules/header_guard.rule +++ /dev/null @@ -1,10 +0,0 @@ - - - define - #define _[A-Z] - - _AZdefine - warning - Definitions should not start with underscore and then a capital letter. - - diff --git a/tools/cppcheck_rules/namespace_AZ.rule b/tools/cppcheck_rules/namespace_AZ.rule deleted file mode 100644 index 75250d7fcf..0000000000 --- a/tools/cppcheck_rules/namespace_AZ.rule +++ /dev/null @@ -1,9 +0,0 @@ - - - :: _[A-Z] - - _AZnaming - warning - Identifier that starts with underscore and then a capital letter is reserved. - - diff --git a/tools/cpplint.py b/tools/cpplint.py deleted file mode 100644 index 9bcd5b3bd0..0000000000 --- a/tools/cpplint.py +++ /dev/null @@ -1,6221 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2009 Google Inc. 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 Google Inc. 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. - -"""Does google-lint on c++ files. - -The goal of this script is to identify places in the code that *may* -be in non-compliance with google style. It does not attempt to fix -up these problems -- the point is to educate. It does also not -attempt to find all problems, or to ensure that everything it does -find is legitimately a problem. - -In particular, we can get very confused by /* and // inside strings! -We do a small hack, which is to ignore //'s with "'s after them on the -same line, but it is far from perfect (in either direction). -""" - -import codecs -import copy -import getopt -import math # for log -import os -import re -import sre_compile -import string -import sys -import unicodedata - - -_USAGE = """ -Syntax: cpplint.py [--verbose=#] [--output=vs7] [--filter=-x,+y,...] - [--counting=total|toplevel|detailed] [--root=subdir] - [--linelength=digits] [--headers=x,y,...] - [--quiet] - [file] ... - - The style guidelines this tries to follow are those in - https://google-styleguide.googlecode.com/svn/trunk/cppguide.xml - - Every problem is given a confidence score from 1-5, with 5 meaning we are - certain of the problem, and 1 meaning it could be a legitimate construct. - This will miss some errors, and is not a substitute for a code review. - - To suppress false-positive errors of a certain category, add a - 'NOLINT(category)' comment to the line. NOLINT or NOLINT(*) - suppresses errors of all categories on that line. - - The files passed in will be linted; at least one file must be provided. - Default linted extensions are .cc, .cpp, .cu, .cuh and .h. Change the - extensions with the --extensions flag. - - Flags: - - output=vs7 - By default, the output is formatted to ease emacs parsing. Visual Studio - compatible output (vs7) may also be used. Other formats are unsupported. - - verbose=# - Specify a number 0-5 to restrict errors to certain verbosity levels. - - quiet - Don't print anything if no errors are found. - - filter=-x,+y,... - Specify a comma-separated list of category-filters to apply: only - error messages whose category names pass the filters will be printed. - (Category names are printed with the message and look like - "[whitespace/indent]".) Filters are evaluated left to right. - "-FOO" and "FOO" means "do not print categories that start with FOO". - "+FOO" means "do print categories that start with FOO". - - Examples: --filter=-whitespace,+whitespace/braces - --filter=whitespace,runtime/printf,+runtime/printf_format - --filter=-,+build/include_what_you_use - - To see a list of all the categories used in cpplint, pass no arg: - --filter= - - counting=total|toplevel|detailed - The total number of errors found is always printed. If - 'toplevel' is provided, then the count of errors in each of - the top-level categories like 'build' and 'whitespace' will - also be printed. If 'detailed' is provided, then a count - is provided for each category like 'build/class'. - - root=subdir - The root directory used for deriving header guard CPP variable. - By default, the header guard CPP variable is calculated as the relative - path to the directory that contains .git, .hg, or .svn. When this flag - is specified, the relative path is calculated from the specified - directory. If the specified directory does not exist, this flag is - ignored. - - Examples: - Assuming that top/src/.git exists (and cwd=top/src), the header guard - CPP variables for top/src/chrome/browser/ui/browser.h are: - - No flag => CHROME_BROWSER_UI_BROWSER_H_ - --root=chrome => BROWSER_UI_BROWSER_H_ - --root=chrome/browser => UI_BROWSER_H_ - --root=.. => SRC_CHROME_BROWSER_UI_BROWSER_H_ - - linelength=digits - This is the allowed line length for the project. The default value is - 80 characters. - - Examples: - --linelength=120 - - extensions=extension,extension,... - The allowed file extensions that cpplint will check - - Examples: - --extensions=hpp,cpp - - headers=x,y,... - The header extensions that cpplint will treat as .h in checks. Values are - automatically added to --extensions list. - - Examples: - --headers=hpp,hxx - --headers=hpp - - cpplint.py supports per-directory configurations specified in CPPLINT.cfg - files. CPPLINT.cfg file can contain a number of key=value pairs. - Currently the following options are supported: - - set noparent - filter=+filter1,-filter2,... - exclude_files=regex - linelength=80 - root=subdir - headers=x,y,... - - "set noparent" option prevents cpplint from traversing directory tree - upwards looking for more .cfg files in parent directories. This option - is usually placed in the top-level project directory. - - The "filter" option is similar in function to --filter flag. It specifies - message filters in addition to the |_DEFAULT_FILTERS| and those specified - through --filter command-line flag. - - "exclude_files" allows to specify a regular expression to be matched against - a file name. If the expression matches, the file is skipped and not run - through liner. - - "linelength" allows to specify the allowed line length for the project. - - The "root" option is similar in function to the --root flag (see example - above). Paths are relative to the directory of the CPPLINT.cfg. - - The "headers" option is similar in function to the --headers flag - (see example above). - - CPPLINT.cfg has an effect on files in the same directory and all - sub-directories, unless overridden by a nested configuration file. - - Example file: - filter=-build/include_order,+build/include_alpha - exclude_files=.*\.cc - - The above example disables build/include_order warning and enables - build/include_alpha as well as excludes all .cc from being - processed by linter, in the current directory (where the .cfg - file is located) and all sub-directories. -""" - -# We categorize each error message we print. Here are the categories. -# We want an explicit list so we can list them all in cpplint --filter=. -# If you add a new error message with a new category, add it to the list -# here! cpplint_unittest.py should tell you if you forget to do this. -_ERROR_CATEGORIES = [ - 'build/class', - 'build/c++11', - 'build/c++14', - 'build/c++tr1', - 'build/deprecated', - 'build/endif_comment', - 'build/explicit_make_pair', - 'build/forward_decl', - 'build/header_guard', - 'build/include', - 'build/include_alpha', - 'build/include_order', - 'build/include_what_you_use', - 'build/namespaces', - 'build/printf_format', - 'build/storage_class', - 'legal/copyright', - 'readability/alt_tokens', - 'readability/braces', - 'readability/casting', - 'readability/check', - 'readability/constructors', - 'readability/fn_size', - 'readability/inheritance', - 'readability/multiline_comment', - 'readability/multiline_string', - 'readability/namespace', - 'readability/nolint', - 'readability/nul', - 'readability/strings', - 'readability/todo', - 'readability/utf8', - 'runtime/arrays', - 'runtime/casting', - 'runtime/explicit', - 'runtime/int', - 'runtime/init', - 'runtime/invalid_increment', - 'runtime/member_string_references', - 'runtime/memset', - 'runtime/indentation_namespace', - 'runtime/operator', - 'runtime/printf', - 'runtime/printf_format', - 'runtime/references', - 'runtime/string', - 'runtime/threadsafe_fn', - 'runtime/vlog', - 'whitespace/blank_line', - 'whitespace/braces', - 'whitespace/comma', - 'whitespace/comments', - 'whitespace/empty_conditional_body', - 'whitespace/empty_if_body', - 'whitespace/empty_loop_body', - 'whitespace/end_of_line', - 'whitespace/ending_newline', - 'whitespace/forcolon', - 'whitespace/indent', - 'whitespace/line_length', - 'whitespace/newline', - 'whitespace/operators', - 'whitespace/parens', - 'whitespace/semicolon', - 'whitespace/tab', - 'whitespace/todo', - ] - -# These error categories are no longer enforced by cpplint, but for backwards- -# compatibility they may still appear in NOLINT comments. -_LEGACY_ERROR_CATEGORIES = [ - 'readability/streams', - 'readability/function', - ] - -# The default state of the category filter. This is overridden by the --filter= -# flag. By default all errors are on, so only add here categories that should be -# off by default (i.e., categories that must be enabled by the --filter= flags). -# All entries here should start with a '-' or '+', as in the --filter= flag. -_DEFAULT_FILTERS = ['-build/include_alpha'] - -# The default list of categories suppressed for C (not C++) files. -_DEFAULT_C_SUPPRESSED_CATEGORIES = [ - 'readability/casting', - ] - -# The default list of categories suppressed for Linux Kernel files. -_DEFAULT_KERNEL_SUPPRESSED_CATEGORIES = [ - 'whitespace/tab', - ] - -# We used to check for high-bit characters, but after much discussion we -# decided those were OK, as long as they were in UTF-8 and didn't represent -# hard-coded international strings, which belong in a separate i18n file. - -# C++ headers -_CPP_HEADERS = frozenset([ - # Legacy - 'algobase.h', - 'algo.h', - 'alloc.h', - 'builtinbuf.h', - 'bvector.h', - 'complex.h', - 'defalloc.h', - 'deque.h', - 'editbuf.h', - 'fstream.h', - 'function.h', - 'hash_map', - 'hash_map.h', - 'hash_set', - 'hash_set.h', - 'hashtable.h', - 'heap.h', - 'indstream.h', - 'iomanip.h', - 'iostream.h', - 'istream.h', - 'iterator.h', - 'list.h', - 'map.h', - 'multimap.h', - 'multiset.h', - 'ostream.h', - 'pair.h', - 'parsestream.h', - 'pfstream.h', - 'procbuf.h', - 'pthread_alloc', - 'pthread_alloc.h', - 'rope', - 'rope.h', - 'ropeimpl.h', - 'set.h', - 'slist', - 'slist.h', - 'stack.h', - 'stdiostream.h', - 'stl_alloc.h', - 'stl_relops.h', - 'streambuf.h', - 'stream.h', - 'strfile.h', - 'strstream.h', - 'tempbuf.h', - 'tree.h', - 'type_traits.h', - 'vector.h', - # 17.6.1.2 C++ library headers - 'algorithm', - 'array', - 'atomic', - 'bitset', - 'chrono', - 'codecvt', - 'complex', - 'condition_variable', - 'deque', - 'exception', - 'forward_list', - 'fstream', - 'functional', - 'future', - 'initializer_list', - 'iomanip', - 'ios', - 'iosfwd', - 'iostream', - 'istream', - 'iterator', - 'limits', - 'list', - 'locale', - 'map', - 'memory', - 'mutex', - 'new', - 'numeric', - 'ostream', - 'queue', - 'random', - 'ratio', - 'regex', - 'scoped_allocator', - 'set', - 'sstream', - 'stack', - 'stdexcept', - 'streambuf', - 'string', - 'strstream', - 'system_error', - 'thread', - 'tuple', - 'typeindex', - 'typeinfo', - 'type_traits', - 'unordered_map', - 'unordered_set', - 'utility', - 'valarray', - 'vector', - # 17.6.1.2 C++14 headers - 'shared_mutex', - # 17.6.1.2 C++17 headers - 'any', - 'charconv', - 'codecvt', - 'execution', - 'filesystem', - 'memory_resource', - 'optional', - 'string_view', - 'variant', - # 17.6.1.2 C++ headers for C library facilities - 'cassert', - 'ccomplex', - 'cctype', - 'cerrno', - 'cfenv', - 'cfloat', - 'cinttypes', - 'ciso646', - 'climits', - 'clocale', - 'cmath', - 'csetjmp', - 'csignal', - 'cstdalign', - 'cstdarg', - 'cstdbool', - 'cstddef', - 'cstdint', - 'cstdio', - 'cstdlib', - 'cstring', - 'ctgmath', - 'ctime', - 'cuchar', - 'cwchar', - 'cwctype', - ]) - -# Type names -_TYPES = re.compile( - r'^(?:' - # [dcl.type.simple] - r'(char(16_t|32_t)?)|wchar_t|' - r'bool|short|int|long|signed|unsigned|float|double|' - # [support.types] - r'(ptrdiff_t|size_t|max_align_t|nullptr_t)|' - # [cstdint.syn] - r'(u?int(_fast|_least)?(8|16|32|64)_t)|' - r'(u?int(max|ptr)_t)|' - r')$') - - -# These headers are excluded from [build/include] and [build/include_order] -# checks: -# - Anything not following google file name conventions (containing an -# uppercase character, such as Python.h or nsStringAPI.h, for example). -# - Lua headers. -_THIRD_PARTY_HEADERS_PATTERN = re.compile( - r'^(?:[^/]*[A-Z][^/]*\.h|lua\.h|lauxlib\.h|lualib\.h)$') - -# Pattern for matching FileInfo.BaseName() against test file name -_TEST_FILE_SUFFIX = r'(_test|_unittest|_regtest)$' - -# Pattern that matches only complete whitespace, possibly across multiple lines. -_EMPTY_CONDITIONAL_BODY_PATTERN = re.compile(r'^\s*$', re.DOTALL) - -# Assertion macros. These are defined in base/logging.h and -# testing/base/public/gunit.h. -_CHECK_MACROS = [ - 'DCHECK', 'CHECK', - 'EXPECT_TRUE', 'ASSERT_TRUE', - 'EXPECT_FALSE', 'ASSERT_FALSE', - ] - -# Replacement macros for CHECK/DCHECK/EXPECT_TRUE/EXPECT_FALSE -_CHECK_REPLACEMENT = dict([(m, {}) for m in _CHECK_MACROS]) - -for op, replacement in [('==', 'EQ'), ('!=', 'NE'), - ('>=', 'GE'), ('>', 'GT'), - ('<=', 'LE'), ('<', 'LT')]: - _CHECK_REPLACEMENT['DCHECK'][op] = 'DCHECK_%s' % replacement - _CHECK_REPLACEMENT['CHECK'][op] = 'CHECK_%s' % replacement - _CHECK_REPLACEMENT['EXPECT_TRUE'][op] = 'EXPECT_%s' % replacement - _CHECK_REPLACEMENT['ASSERT_TRUE'][op] = 'ASSERT_%s' % replacement - -for op, inv_replacement in [('==', 'NE'), ('!=', 'EQ'), - ('>=', 'LT'), ('>', 'LE'), - ('<=', 'GT'), ('<', 'GE')]: - _CHECK_REPLACEMENT['EXPECT_FALSE'][op] = 'EXPECT_%s' % inv_replacement - _CHECK_REPLACEMENT['ASSERT_FALSE'][op] = 'ASSERT_%s' % inv_replacement - -# Alternative tokens and their replacements. For full list, see section 2.5 -# Alternative tokens [lex.digraph] in the C++ standard. -# -# Digraphs (such as '%:') are not included here since it's a mess to -# match those on a word boundary. -_ALT_TOKEN_REPLACEMENT = { - 'and': '&&', - 'bitor': '|', - 'or': '||', - 'xor': '^', - 'compl': '~', - 'bitand': '&', - 'and_eq': '&=', - 'or_eq': '|=', - 'xor_eq': '^=', - 'not': '!', - 'not_eq': '!=' - } - -# Compile regular expression that matches all the above keywords. The "[ =()]" -# bit is meant to avoid matching these keywords outside of boolean expressions. -# -# False positives include C-style multi-line comments and multi-line strings -# but those have always been troublesome for cpplint. -_ALT_TOKEN_REPLACEMENT_PATTERN = re.compile( - r'[ =()](' + ('|'.join(_ALT_TOKEN_REPLACEMENT.keys())) + r')(?=[ (]|$)') - - -# These constants define types of headers for use with -# _IncludeState.CheckNextIncludeOrder(). -_C_SYS_HEADER = 1 -_CPP_SYS_HEADER = 2 -_LIKELY_MY_HEADER = 3 -_POSSIBLE_MY_HEADER = 4 -_OTHER_HEADER = 5 - -# These constants define the current inline assembly state -_NO_ASM = 0 # Outside of inline assembly block -_INSIDE_ASM = 1 # Inside inline assembly block -_END_ASM = 2 # Last line of inline assembly block -_BLOCK_ASM = 3 # The whole block is an inline assembly block - -# Match start of assembly blocks -_MATCH_ASM = re.compile(r'^\s*(?:asm|_asm|__asm|__asm__)' - r'(?:\s+(volatile|__volatile__))?' - r'\s*[{(]') - -# Match strings that indicate we're working on a C (not C++) file. -_SEARCH_C_FILE = re.compile(r'\b(?:LINT_C_FILE|' - r'vim?:\s*.*(\s*|:)filetype=c(\s*|:|$))') - -# Match string that indicates we're working on a Linux Kernel file. -_SEARCH_KERNEL_FILE = re.compile(r'\b(?:LINT_KERNEL_FILE)') - -_regexp_compile_cache = {} - -# {str, set(int)}: a map from error categories to sets of linenumbers -# on which those errors are expected and should be suppressed. -_error_suppressions = {} - -# The root directory used for deriving header guard CPP variable. -# This is set by --root flag. -_root = None -_root_debug = False - -# The allowed line length of files. -# This is set by --linelength flag. -_line_length = 80 - -# The allowed extensions for file names -# This is set by --extensions flag. -_valid_extensions = set(['cc', 'h', 'cpp', 'cu', 'cuh']) - -# Treat all headers starting with 'h' equally: .h, .hpp, .hxx etc. -# This is set by --headers flag. -_hpp_headers = set(['h']) - -# {str, bool}: a map from error categories to booleans which indicate if the -# category should be suppressed for every line. -_global_error_suppressions = {} - -def ProcessHppHeadersOption(val): - global _hpp_headers - try: - _hpp_headers = set(val.split(',')) - # Automatically append to extensions list so it does not have to be set 2 times - _valid_extensions.update(_hpp_headers) - except ValueError: - PrintUsage('Header extensions must be comma seperated list.') - -def IsHeaderExtension(file_extension): - return file_extension in _hpp_headers - -def ParseNolintSuppressions(filename, raw_line, linenum, error): - """Updates the global list of line error-suppressions. - - Parses any NOLINT comments on the current line, updating the global - error_suppressions store. Reports an error if the NOLINT comment - was malformed. - - Args: - filename: str, the name of the input file. - raw_line: str, the line of input text, with comments. - linenum: int, the number of the current line. - error: function, an error handler. - """ - matched = Search(r'\bNOLINT(NEXTLINE)?\b(\([^)]+\))?', raw_line) - if matched: - if matched.group(1): - suppressed_line = linenum + 1 - else: - suppressed_line = linenum - category = matched.group(2) - if category in (None, '(*)'): # => "suppress all" - _error_suppressions.setdefault(None, set()).add(suppressed_line) - else: - if category.startswith('(') and category.endswith(')'): - category = category[1:-1] - if category in _ERROR_CATEGORIES: - _error_suppressions.setdefault(category, set()).add(suppressed_line) - elif category not in _LEGACY_ERROR_CATEGORIES: - error(filename, linenum, 'readability/nolint', 5, - 'Unknown NOLINT error category: %s' % category) - - -def ProcessGlobalSuppresions(lines): - """Updates the list of global error suppressions. - - Parses any lint directives in the file that have global effect. - - Args: - lines: An array of strings, each representing a line of the file, with the - last element being empty if the file is terminated with a newline. - """ - for line in lines: - if _SEARCH_C_FILE.search(line): - for category in _DEFAULT_C_SUPPRESSED_CATEGORIES: - _global_error_suppressions[category] = True - if _SEARCH_KERNEL_FILE.search(line): - for category in _DEFAULT_KERNEL_SUPPRESSED_CATEGORIES: - _global_error_suppressions[category] = True - - -def ResetNolintSuppressions(): - """Resets the set of NOLINT suppressions to empty.""" - _error_suppressions.clear() - _global_error_suppressions.clear() - - -def IsErrorSuppressedByNolint(category, linenum): - """Returns true if the specified error category is suppressed on this line. - - Consults the global error_suppressions map populated by - ParseNolintSuppressions/ProcessGlobalSuppresions/ResetNolintSuppressions. - - Args: - category: str, the category of the error. - linenum: int, the current line number. - Returns: - bool, True iff the error should be suppressed due to a NOLINT comment or - global suppression. - """ - return (_global_error_suppressions.get(category, False) or - linenum in _error_suppressions.get(category, set()) or - linenum in _error_suppressions.get(None, set())) - - -def Match(pattern, s): - """Matches the string with the pattern, caching the compiled regexp.""" - # The regexp compilation caching is inlined in both Match and Search for - # performance reasons; factoring it out into a separate function turns out - # to be noticeably expensive. - if pattern not in _regexp_compile_cache: - _regexp_compile_cache[pattern] = sre_compile.compile(pattern) - return _regexp_compile_cache[pattern].match(s) - - -def ReplaceAll(pattern, rep, s): - """Replaces instances of pattern in a string with a replacement. - - The compiled regex is kept in a cache shared by Match and Search. - - Args: - pattern: regex pattern - rep: replacement text - s: search string - - Returns: - string with replacements made (or original string if no replacements) - """ - if pattern not in _regexp_compile_cache: - _regexp_compile_cache[pattern] = sre_compile.compile(pattern) - return _regexp_compile_cache[pattern].sub(rep, s) - - -def Search(pattern, s): - """Searches the string for the pattern, caching the compiled regexp.""" - if pattern not in _regexp_compile_cache: - _regexp_compile_cache[pattern] = sre_compile.compile(pattern) - return _regexp_compile_cache[pattern].search(s) - - -def _IsSourceExtension(s): - """File extension (excluding dot) matches a source file extension.""" - return s in ('c', 'cc', 'cpp', 'cxx') - - -class _IncludeState(object): - """Tracks line numbers for includes, and the order in which includes appear. - - include_list contains list of lists of (header, line number) pairs. - It's a lists of lists rather than just one flat list to make it - easier to update across preprocessor boundaries. - - Call CheckNextIncludeOrder() once for each header in the file, passing - in the type constants defined above. Calls in an illegal order will - raise an _IncludeError with an appropriate error message. - - """ - # self._section will move monotonically through this set. If it ever - # needs to move backwards, CheckNextIncludeOrder will raise an error. - _INITIAL_SECTION = 0 - _MY_H_SECTION = 1 - _C_SECTION = 2 - _CPP_SECTION = 3 - _OTHER_H_SECTION = 4 - - _TYPE_NAMES = { - _C_SYS_HEADER: 'C system header', - _CPP_SYS_HEADER: 'C++ system header', - _LIKELY_MY_HEADER: 'header this file implements', - _POSSIBLE_MY_HEADER: 'header this file may implement', - _OTHER_HEADER: 'other header', - } - _SECTION_NAMES = { - _INITIAL_SECTION: "... nothing. (This can't be an error.)", - _MY_H_SECTION: 'a header this file implements', - _C_SECTION: 'C system header', - _CPP_SECTION: 'C++ system header', - _OTHER_H_SECTION: 'other header', - } - - def __init__(self): - self.include_list = [[]] - self.ResetSection('') - - def FindHeader(self, header): - """Check if a header has already been included. - - Args: - header: header to check. - Returns: - Line number of previous occurrence, or -1 if the header has not - been seen before. - """ - for section_list in self.include_list: - for f in section_list: - if f[0] == header: - return f[1] - return -1 - - def ResetSection(self, directive): - """Reset section checking for preprocessor directive. - - Args: - directive: preprocessor directive (e.g. "if", "else"). - """ - # The name of the current section. - self._section = self._INITIAL_SECTION - # The path of last found header. - self._last_header = '' - - # Update list of includes. Note that we never pop from the - # include list. - if directive in ('if', 'ifdef', 'ifndef'): - self.include_list.append([]) - elif directive in ('else', 'elif'): - self.include_list[-1] = [] - - def SetLastHeader(self, header_path): - self._last_header = header_path - - def CanonicalizeAlphabeticalOrder(self, header_path): - """Returns a path canonicalized for alphabetical comparison. - - - replaces "-" with "_" so they both cmp the same. - - removes '-inl' since we don't require them to be after the main header. - - lowercase everything, just in case. - - Args: - header_path: Path to be canonicalized. - - Returns: - Canonicalized path. - """ - return header_path.replace('-inl.h', '.h').replace('-', '_').lower() - - def IsInAlphabeticalOrder(self, clean_lines, linenum, header_path): - """Check if a header is in alphabetical order with the previous header. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - header_path: Canonicalized header to be checked. - - Returns: - Returns true if the header is in alphabetical order. - """ - # If previous section is different from current section, _last_header will - # be reset to empty string, so it's always less than current header. - # - # If previous line was a blank line, assume that the headers are - # intentionally sorted the way they are. - if (self._last_header > header_path and - Match(r'^\s*#\s*include\b', clean_lines.elided[linenum - 1])): - return False - return True - - def CheckNextIncludeOrder(self, header_type): - """Returns a non-empty error message if the next header is out of order. - - This function also updates the internal state to be ready to check - the next include. - - Args: - header_type: One of the _XXX_HEADER constants defined above. - - Returns: - The empty string if the header is in the right order, or an - error message describing what's wrong. - - """ - error_message = ('Found %s after %s' % - (self._TYPE_NAMES[header_type], - self._SECTION_NAMES[self._section])) - - last_section = self._section - - if header_type == _C_SYS_HEADER: - if self._section <= self._C_SECTION: - self._section = self._C_SECTION - else: - self._last_header = '' - return error_message - elif header_type == _CPP_SYS_HEADER: - if self._section <= self._CPP_SECTION: - self._section = self._CPP_SECTION - else: - self._last_header = '' - return error_message - elif header_type == _LIKELY_MY_HEADER: - if self._section <= self._MY_H_SECTION: - self._section = self._MY_H_SECTION - else: - self._section = self._OTHER_H_SECTION - elif header_type == _POSSIBLE_MY_HEADER: - if self._section <= self._MY_H_SECTION: - self._section = self._MY_H_SECTION - else: - # This will always be the fallback because we're not sure - # enough that the header is associated with this file. - self._section = self._OTHER_H_SECTION - else: - assert header_type == _OTHER_HEADER - self._section = self._OTHER_H_SECTION - - if last_section != self._section: - self._last_header = '' - - return '' - - -class _CppLintState(object): - """Maintains module-wide state..""" - - def __init__(self): - self.verbose_level = 1 # global setting. - self.error_count = 0 # global count of reported errors - # filters to apply when emitting error messages - self.filters = _DEFAULT_FILTERS[:] - # backup of filter list. Used to restore the state after each file. - self._filters_backup = self.filters[:] - self.counting = 'total' # In what way are we counting errors? - self.errors_by_category = {} # string to int dict storing error counts - self.quiet = False # Suppress non-error messagess? - - # output format: - # "emacs" - format that emacs can parse (default) - # "vs7" - format that Microsoft Visual Studio 7 can parse - self.output_format = 'emacs' - - def SetOutputFormat(self, output_format): - """Sets the output format for errors.""" - self.output_format = output_format - - def SetQuiet(self, quiet): - """Sets the module's quiet settings, and returns the previous setting.""" - last_quiet = self.quiet - self.quiet = quiet - return last_quiet - - def SetVerboseLevel(self, level): - """Sets the module's verbosity, and returns the previous setting.""" - last_verbose_level = self.verbose_level - self.verbose_level = level - return last_verbose_level - - def SetCountingStyle(self, counting_style): - """Sets the module's counting options.""" - self.counting = counting_style - - def SetFilters(self, filters): - """Sets the error-message filters. - - These filters are applied when deciding whether to emit a given - error message. - - Args: - filters: A string of comma-separated filters (eg "+whitespace/indent"). - Each filter should start with + or -; else we die. - - Raises: - ValueError: The comma-separated filters did not all start with '+' or '-'. - E.g. "-,+whitespace,-whitespace/indent,whitespace/badfilter" - """ - # Default filters always have less priority than the flag ones. - self.filters = _DEFAULT_FILTERS[:] - self.AddFilters(filters) - - def AddFilters(self, filters): - """ Adds more filters to the existing list of error-message filters. """ - for filt in filters.split(','): - clean_filt = filt.strip() - if clean_filt: - self.filters.append(clean_filt) - for filt in self.filters: - if not (filt.startswith('+') or filt.startswith('-')): - raise ValueError('Every filter in --filters must start with + or -' - ' (%s does not)' % filt) - - def BackupFilters(self): - """ Saves the current filter list to backup storage.""" - self._filters_backup = self.filters[:] - - def RestoreFilters(self): - """ Restores filters previously backed up.""" - self.filters = self._filters_backup[:] - - def ResetErrorCounts(self): - """Sets the module's error statistic back to zero.""" - self.error_count = 0 - self.errors_by_category = {} - - def IncrementErrorCount(self, category): - """Bumps the module's error statistic.""" - self.error_count += 1 - if self.counting in ('toplevel', 'detailed'): - if self.counting != 'detailed': - category = category.split('/')[0] - if category not in self.errors_by_category: - self.errors_by_category[category] = 0 - self.errors_by_category[category] += 1 - - def PrintErrorCounts(self): - """Print a summary of errors by category, and the total.""" - for category, count in self.errors_by_category.iteritems(): - sys.stderr.write('Category \'%s\' errors found: %d\n' % - (category, count)) - sys.stdout.write('Total errors found: %d\n' % self.error_count) - -_cpplint_state = _CppLintState() - - -def _OutputFormat(): - """Gets the module's output format.""" - return _cpplint_state.output_format - - -def _SetOutputFormat(output_format): - """Sets the module's output format.""" - _cpplint_state.SetOutputFormat(output_format) - -def _Quiet(): - """Return's the module's quiet setting.""" - return _cpplint_state.quiet - -def _SetQuiet(quiet): - """Set the module's quiet status, and return previous setting.""" - return _cpplint_state.SetQuiet(quiet) - - -def _VerboseLevel(): - """Returns the module's verbosity setting.""" - return _cpplint_state.verbose_level - - -def _SetVerboseLevel(level): - """Sets the module's verbosity, and returns the previous setting.""" - return _cpplint_state.SetVerboseLevel(level) - - -def _SetCountingStyle(level): - """Sets the module's counting options.""" - _cpplint_state.SetCountingStyle(level) - - -def _Filters(): - """Returns the module's list of output filters, as a list.""" - return _cpplint_state.filters - - -def _SetFilters(filters): - """Sets the module's error-message filters. - - These filters are applied when deciding whether to emit a given - error message. - - Args: - filters: A string of comma-separated filters (eg "whitespace/indent"). - Each filter should start with + or -; else we die. - """ - _cpplint_state.SetFilters(filters) - -def _AddFilters(filters): - """Adds more filter overrides. - - Unlike _SetFilters, this function does not reset the current list of filters - available. - - Args: - filters: A string of comma-separated filters (eg "whitespace/indent"). - Each filter should start with + or -; else we die. - """ - _cpplint_state.AddFilters(filters) - -def _BackupFilters(): - """ Saves the current filter list to backup storage.""" - _cpplint_state.BackupFilters() - -def _RestoreFilters(): - """ Restores filters previously backed up.""" - _cpplint_state.RestoreFilters() - -class _FunctionState(object): - """Tracks current function name and the number of lines in its body.""" - - _NORMAL_TRIGGER = 250 # for --v=0, 500 for --v=1, etc. - _TEST_TRIGGER = 400 # about 50% more than _NORMAL_TRIGGER. - - def __init__(self): - self.in_a_function = False - self.lines_in_function = 0 - self.current_function = '' - - def Begin(self, function_name): - """Start analyzing function body. - - Args: - function_name: The name of the function being tracked. - """ - self.in_a_function = True - self.lines_in_function = 0 - self.current_function = function_name - - def Count(self): - """Count line in current function body.""" - if self.in_a_function: - self.lines_in_function += 1 - - def Check(self, error, filename, linenum): - """Report if too many lines in function body. - - Args: - error: The function to call with any errors found. - filename: The name of the current file. - linenum: The number of the line to check. - """ - if not self.in_a_function: - return - - if Match(r'T(EST|est)', self.current_function): - base_trigger = self._TEST_TRIGGER - else: - base_trigger = self._NORMAL_TRIGGER - trigger = base_trigger * 2**_VerboseLevel() - - if self.lines_in_function > trigger: - error_level = int(math.log(self.lines_in_function / base_trigger, 2)) - # 50 => 0, 100 => 1, 200 => 2, 400 => 3, 800 => 4, 1600 => 5, ... - if error_level > 5: - error_level = 5 - error(filename, linenum, 'readability/fn_size', error_level, - 'Small and focused functions are preferred:' - ' %s has %d non-comment lines' - ' (error triggered by exceeding %d lines).' % ( - self.current_function, self.lines_in_function, trigger)) - - def End(self): - """Stop analyzing function body.""" - self.in_a_function = False - - -class _IncludeError(Exception): - """Indicates a problem with the include order in a file.""" - pass - - -class FileInfo(object): - """Provides utility functions for filenames. - - FileInfo provides easy access to the components of a file's path - relative to the project root. - """ - - def __init__(self, filename): - self._filename = filename - - def FullName(self): - """Make Windows paths like Unix.""" - return os.path.abspath(self._filename).replace('\\', '/') - - def RepositoryName(self): - """FullName after removing the local path to the repository. - - If we have a real absolute path name here we can try to do something smart: - detecting the root of the checkout and truncating /path/to/checkout from - the name so that we get header guards that don't include things like - "C:\Documents and Settings\..." or "/home/username/..." in them and thus - people on different computers who have checked the source out to different - locations won't see bogus errors. - """ - fullname = self.FullName() - - if os.path.exists(fullname): - project_dir = os.path.dirname(fullname) - - if os.path.exists(os.path.join(project_dir, ".svn")): - # If there's a .svn file in the current directory, we recursively look - # up the directory tree for the top of the SVN checkout - root_dir = project_dir - one_up_dir = os.path.dirname(root_dir) - while os.path.exists(os.path.join(one_up_dir, ".svn")): - root_dir = os.path.dirname(root_dir) - one_up_dir = os.path.dirname(one_up_dir) - - prefix = os.path.commonprefix([root_dir, project_dir]) - return fullname[len(prefix) + 1:] - - # Not SVN <= 1.6? Try to find a git, hg, or svn top level directory by - # searching up from the current path. - root_dir = current_dir = os.path.dirname(fullname) - while current_dir != os.path.dirname(current_dir): - if (os.path.exists(os.path.join(current_dir, ".git")) or - os.path.exists(os.path.join(current_dir, ".hg")) or - os.path.exists(os.path.join(current_dir, ".svn"))): - root_dir = current_dir - current_dir = os.path.dirname(current_dir) - - if (os.path.exists(os.path.join(root_dir, ".git")) or - os.path.exists(os.path.join(root_dir, ".hg")) or - os.path.exists(os.path.join(root_dir, ".svn"))): - prefix = os.path.commonprefix([root_dir, project_dir]) - return fullname[len(prefix) + 1:] - - # Don't know what to do; header guard warnings may be wrong... - return fullname - - def Split(self): - """Splits the file into the directory, basename, and extension. - - For 'chrome/browser/browser.cc', Split() would - return ('chrome/browser', 'browser', '.cc') - - Returns: - A tuple of (directory, basename, extension). - """ - - googlename = self.RepositoryName() - project, rest = os.path.split(googlename) - return (project,) + os.path.splitext(rest) - - def BaseName(self): - """File base name - text after the final slash, before the final period.""" - return self.Split()[1] - - def Extension(self): - """File extension - text following the final period.""" - return self.Split()[2] - - def NoExtension(self): - """File has no source file extension.""" - return '/'.join(self.Split()[0:2]) - - def IsSource(self): - """File has a source file extension.""" - return _IsSourceExtension(self.Extension()[1:]) - - -def _ShouldPrintError(category, confidence, linenum): - """If confidence >= verbose, category passes filter and is not suppressed.""" - - # There are three ways we might decide not to print an error message: - # a "NOLINT(category)" comment appears in the source, - # the verbosity level isn't high enough, or the filters filter it out. - if IsErrorSuppressedByNolint(category, linenum): - return False - - if confidence < _cpplint_state.verbose_level: - return False - - is_filtered = False - for one_filter in _Filters(): - if one_filter.startswith('-'): - if category.startswith(one_filter[1:]): - is_filtered = True - elif one_filter.startswith('+'): - if category.startswith(one_filter[1:]): - is_filtered = False - else: - assert False # should have been checked for in SetFilter. - if is_filtered: - return False - - return True - - -def Error(filename, linenum, category, confidence, message): - """Logs the fact we've found a lint error. - - We log where the error was found, and also our confidence in the error, - that is, how certain we are this is a legitimate style regression, and - not a misidentification or a use that's sometimes justified. - - False positives can be suppressed by the use of - "cpplint(category)" comments on the offending line. These are - parsed into _error_suppressions. - - Args: - filename: The name of the file containing the error. - linenum: The number of the line containing the error. - category: A string used to describe the "category" this bug - falls under: "whitespace", say, or "runtime". Categories - may have a hierarchy separated by slashes: "whitespace/indent". - confidence: A number from 1-5 representing a confidence score for - the error, with 5 meaning that we are certain of the problem, - and 1 meaning that it could be a legitimate construct. - message: The error message. - """ - if _ShouldPrintError(category, confidence, linenum): - _cpplint_state.IncrementErrorCount(category) - if _cpplint_state.output_format == 'vs7': - sys.stderr.write('%s(%s): error cpplint: [%s] %s [%d]\n' % ( - filename, linenum, category, message, confidence)) - elif _cpplint_state.output_format == 'eclipse': - sys.stderr.write('%s:%s: warning: %s [%s] [%d]\n' % ( - filename, linenum, message, category, confidence)) - else: - sys.stderr.write('%s:%s: %s [%s] [%d]\n' % ( - filename, linenum, message, category, confidence)) - - -# Matches standard C++ escape sequences per 2.13.2.3 of the C++ standard. -_RE_PATTERN_CLEANSE_LINE_ESCAPES = re.compile( - r'\\([abfnrtv?"\\\']|\d+|x[0-9a-fA-F]+)') -# Match a single C style comment on the same line. -_RE_PATTERN_C_COMMENTS = r'/\*(?:[^*]|\*(?!/))*\*/' -# Matches multi-line C style comments. -# This RE is a little bit more complicated than one might expect, because we -# have to take care of space removals tools so we can handle comments inside -# statements better. -# The current rule is: We only clear spaces from both sides when we're at the -# end of the line. Otherwise, we try to remove spaces from the right side, -# if this doesn't work we try on left side but only if there's a non-character -# on the right. -_RE_PATTERN_CLEANSE_LINE_C_COMMENTS = re.compile( - r'(\s*' + _RE_PATTERN_C_COMMENTS + r'\s*$|' + - _RE_PATTERN_C_COMMENTS + r'\s+|' + - r'\s+' + _RE_PATTERN_C_COMMENTS + r'(?=\W)|' + - _RE_PATTERN_C_COMMENTS + r')') - - -def IsCppString(line): - """Does line terminate so, that the next symbol is in string constant. - - This function does not consider single-line nor multi-line comments. - - Args: - line: is a partial line of code starting from the 0..n. - - Returns: - True, if next character appended to 'line' is inside a - string constant. - """ - - line = line.replace(r'\\', 'XX') # after this, \\" does not match to \" - return ((line.count('"') - line.count(r'\"') - line.count("'\"'")) & 1) == 1 - - -def CleanseRawStrings(raw_lines): - """Removes C++11 raw strings from lines. - - Before: - static const char kData[] = R"( - multi-line string - )"; - - After: - static const char kData[] = "" - (replaced by blank line) - ""; - - Args: - raw_lines: list of raw lines. - - Returns: - list of lines with C++11 raw strings replaced by empty strings. - """ - - delimiter = None - lines_without_raw_strings = [] - for line in raw_lines: - if delimiter: - # Inside a raw string, look for the end - end = line.find(delimiter) - if end >= 0: - # Found the end of the string, match leading space for this - # line and resume copying the original lines, and also insert - # a "" on the last line. - leading_space = Match(r'^(\s*)\S', line) - line = leading_space.group(1) + '""' + line[end + len(delimiter):] - delimiter = None - else: - # Haven't found the end yet, append a blank line. - line = '""' - - # Look for beginning of a raw string, and replace them with - # empty strings. This is done in a loop to handle multiple raw - # strings on the same line. - while delimiter is None: - # Look for beginning of a raw string. - # See 2.14.15 [lex.string] for syntax. - # - # Once we have matched a raw string, we check the prefix of the - # line to make sure that the line is not part of a single line - # comment. It's done this way because we remove raw strings - # before removing comments as opposed to removing comments - # before removing raw strings. This is because there are some - # cpplint checks that requires the comments to be preserved, but - # we don't want to check comments that are inside raw strings. - matched = Match(r'^(.*?)\b(?:R|u8R|uR|UR|LR)"([^\s\\()]*)\((.*)$', line) - if (matched and - not Match(r'^([^\'"]|\'(\\.|[^\'])*\'|"(\\.|[^"])*")*//', - matched.group(1))): - delimiter = ')' + matched.group(2) + '"' - - end = matched.group(3).find(delimiter) - if end >= 0: - # Raw string ended on same line - line = (matched.group(1) + '""' + - matched.group(3)[end + len(delimiter):]) - delimiter = None - else: - # Start of a multi-line raw string - line = matched.group(1) + '""' - else: - break - - lines_without_raw_strings.append(line) - - # TODO(unknown): if delimiter is not None here, we might want to - # emit a warning for unterminated string. - return lines_without_raw_strings - - -def FindNextMultiLineCommentStart(lines, lineix): - """Find the beginning marker for a multiline comment.""" - while lineix < len(lines): - if lines[lineix].strip().startswith('/*'): - # Only return this marker if the comment goes beyond this line - if lines[lineix].strip().find('*/', 2) < 0: - return lineix - lineix += 1 - return len(lines) - - -def FindNextMultiLineCommentEnd(lines, lineix): - """We are inside a comment, find the end marker.""" - while lineix < len(lines): - if lines[lineix].strip().endswith('*/'): - return lineix - lineix += 1 - return len(lines) - - -def RemoveMultiLineCommentsFromRange(lines, begin, end): - """Clears a range of lines for multi-line comments.""" - # Having // dummy comments makes the lines non-empty, so we will not get - # unnecessary blank line warnings later in the code. - for i in range(begin, end): - lines[i] = '/**/' - - -def RemoveMultiLineComments(filename, lines, error): - """Removes multiline (c-style) comments from lines.""" - lineix = 0 - while lineix < len(lines): - lineix_begin = FindNextMultiLineCommentStart(lines, lineix) - if lineix_begin >= len(lines): - return - lineix_end = FindNextMultiLineCommentEnd(lines, lineix_begin) - if lineix_end >= len(lines): - error(filename, lineix_begin + 1, 'readability/multiline_comment', 5, - 'Could not find end of multi-line comment') - return - RemoveMultiLineCommentsFromRange(lines, lineix_begin, lineix_end + 1) - lineix = lineix_end + 1 - - -def CleanseComments(line): - """Removes //-comments and single-line C-style /* */ comments. - - Args: - line: A line of C++ source. - - Returns: - The line with single-line comments removed. - """ - commentpos = line.find('//') - if commentpos != -1 and not IsCppString(line[:commentpos]): - line = line[:commentpos].rstrip() - # get rid of /* ... */ - return _RE_PATTERN_CLEANSE_LINE_C_COMMENTS.sub('', line) - - -class CleansedLines(object): - """Holds 4 copies of all lines with different preprocessing applied to them. - - 1) elided member contains lines without strings and comments. - 2) lines member contains lines without comments. - 3) raw_lines member contains all the lines without processing. - 4) lines_without_raw_strings member is same as raw_lines, but with C++11 raw - strings removed. - All these members are of , and of the same length. - """ - - def __init__(self, lines): - self.elided = [] - self.lines = [] - self.raw_lines = lines - self.num_lines = len(lines) - self.lines_without_raw_strings = CleanseRawStrings(lines) - for linenum in range(len(self.lines_without_raw_strings)): - self.lines.append(CleanseComments( - self.lines_without_raw_strings[linenum])) - elided = self._CollapseStrings(self.lines_without_raw_strings[linenum]) - self.elided.append(CleanseComments(elided)) - - def NumLines(self): - """Returns the number of lines represented.""" - return self.num_lines - - @staticmethod - def _CollapseStrings(elided): - """Collapses strings and chars on a line to simple "" or '' blocks. - - We nix strings first so we're not fooled by text like '"http://"' - - Args: - elided: The line being processed. - - Returns: - The line with collapsed strings. - """ - if _RE_PATTERN_INCLUDE.match(elided): - return elided - - # Remove escaped characters first to make quote/single quote collapsing - # basic. Things that look like escaped characters shouldn't occur - # outside of strings and chars. - elided = _RE_PATTERN_CLEANSE_LINE_ESCAPES.sub('', elided) - - # Replace quoted strings and digit separators. Both single quotes - # and double quotes are processed in the same loop, otherwise - # nested quotes wouldn't work. - collapsed = '' - while True: - # Find the first quote character - match = Match(r'^([^\'"]*)([\'"])(.*)$', elided) - if not match: - collapsed += elided - break - head, quote, tail = match.groups() - - if quote == '"': - # Collapse double quoted strings - second_quote = tail.find('"') - if second_quote >= 0: - collapsed += head + '""' - elided = tail[second_quote + 1:] - else: - # Unmatched double quote, don't bother processing the rest - # of the line since this is probably a multiline string. - collapsed += elided - break - else: - # Found single quote, check nearby text to eliminate digit separators. - # - # There is no special handling for floating point here, because - # the integer/fractional/exponent parts would all be parsed - # correctly as long as there are digits on both sides of the - # separator. So we are fine as long as we don't see something - # like "0.'3" (gcc 4.9.0 will not allow this literal). - if Search(r'\b(?:0[bBxX]?|[1-9])[0-9a-fA-F]*$', head): - match_literal = Match(r'^((?:\'?[0-9a-zA-Z_])*)(.*)$', "'" + tail) - collapsed += head + match_literal.group(1).replace("'", '') - elided = match_literal.group(2) - else: - second_quote = tail.find('\'') - if second_quote >= 0: - collapsed += head + "''" - elided = tail[second_quote + 1:] - else: - # Unmatched single quote - collapsed += elided - break - - return collapsed - - -def FindEndOfExpressionInLine(line, startpos, stack): - """Find the position just after the end of current parenthesized expression. - - Args: - line: a CleansedLines line. - startpos: start searching at this position. - stack: nesting stack at startpos. - - Returns: - On finding matching end: (index just after matching end, None) - On finding an unclosed expression: (-1, None) - Otherwise: (-1, new stack at end of this line) - """ - for i in xrange(startpos, len(line)): - char = line[i] - if char in '([{': - # Found start of parenthesized expression, push to expression stack - stack.append(char) - elif char == '<': - # Found potential start of template argument list - if i > 0 and line[i - 1] == '<': - # Left shift operator - if stack and stack[-1] == '<': - stack.pop() - if not stack: - return (-1, None) - elif i > 0 and Search(r'\boperator\s*$', line[0:i]): - # operator<, don't add to stack - continue - else: - # Tentative start of template argument list - stack.append('<') - elif char in ')]}': - # Found end of parenthesized expression. - # - # If we are currently expecting a matching '>', the pending '<' - # must have been an operator. Remove them from expression stack. - while stack and stack[-1] == '<': - stack.pop() - if not stack: - return (-1, None) - if ((stack[-1] == '(' and char == ')') or - (stack[-1] == '[' and char == ']') or - (stack[-1] == '{' and char == '}')): - stack.pop() - if not stack: - return (i + 1, None) - else: - # Mismatched parentheses - return (-1, None) - elif char == '>': - # Found potential end of template argument list. - - # Ignore "->" and operator functions - if (i > 0 and - (line[i - 1] == '-' or Search(r'\boperator\s*$', line[0:i - 1]))): - continue - - # Pop the stack if there is a matching '<'. Otherwise, ignore - # this '>' since it must be an operator. - if stack: - if stack[-1] == '<': - stack.pop() - if not stack: - return (i + 1, None) - elif char == ';': - # Found something that look like end of statements. If we are currently - # expecting a '>', the matching '<' must have been an operator, since - # template argument list should not contain statements. - while stack and stack[-1] == '<': - stack.pop() - if not stack: - return (-1, None) - - # Did not find end of expression or unbalanced parentheses on this line - return (-1, stack) - - -def CloseExpression(clean_lines, linenum, pos): - """If input points to ( or { or [ or <, finds the position that closes it. - - If lines[linenum][pos] points to a '(' or '{' or '[' or '<', finds the - linenum/pos that correspond to the closing of the expression. - - TODO(unknown): cpplint spends a fair bit of time matching parentheses. - Ideally we would want to index all opening and closing parentheses once - and have CloseExpression be just a simple lookup, but due to preprocessor - tricks, this is not so easy. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - pos: A position on the line. - - Returns: - A tuple (line, linenum, pos) pointer *past* the closing brace, or - (line, len(lines), -1) if we never find a close. Note we ignore - strings and comments when matching; and the line we return is the - 'cleansed' line at linenum. - """ - - line = clean_lines.elided[linenum] - if (line[pos] not in '({[<') or Match(r'<[<=]', line[pos:]): - return (line, clean_lines.NumLines(), -1) - - # Check first line - (end_pos, stack) = FindEndOfExpressionInLine(line, pos, []) - if end_pos > -1: - return (line, linenum, end_pos) - - # Continue scanning forward - while stack and linenum < clean_lines.NumLines() - 1: - linenum += 1 - line = clean_lines.elided[linenum] - (end_pos, stack) = FindEndOfExpressionInLine(line, 0, stack) - if end_pos > -1: - return (line, linenum, end_pos) - - # Did not find end of expression before end of file, give up - return (line, clean_lines.NumLines(), -1) - - -def FindStartOfExpressionInLine(line, endpos, stack): - """Find position at the matching start of current expression. - - This is almost the reverse of FindEndOfExpressionInLine, but note - that the input position and returned position differs by 1. - - Args: - line: a CleansedLines line. - endpos: start searching at this position. - stack: nesting stack at endpos. - - Returns: - On finding matching start: (index at matching start, None) - On finding an unclosed expression: (-1, None) - Otherwise: (-1, new stack at beginning of this line) - """ - i = endpos - while i >= 0: - char = line[i] - if char in ')]}': - # Found end of expression, push to expression stack - stack.append(char) - elif char == '>': - # Found potential end of template argument list. - # - # Ignore it if it's a "->" or ">=" or "operator>" - if (i > 0 and - (line[i - 1] == '-' or - Match(r'\s>=\s', line[i - 1:]) or - Search(r'\boperator\s*$', line[0:i]))): - i -= 1 - else: - stack.append('>') - elif char == '<': - # Found potential start of template argument list - if i > 0 and line[i - 1] == '<': - # Left shift operator - i -= 1 - else: - # If there is a matching '>', we can pop the expression stack. - # Otherwise, ignore this '<' since it must be an operator. - if stack and stack[-1] == '>': - stack.pop() - if not stack: - return (i, None) - elif char in '([{': - # Found start of expression. - # - # If there are any unmatched '>' on the stack, they must be - # operators. Remove those. - while stack and stack[-1] == '>': - stack.pop() - if not stack: - return (-1, None) - if ((char == '(' and stack[-1] == ')') or - (char == '[' and stack[-1] == ']') or - (char == '{' and stack[-1] == '}')): - stack.pop() - if not stack: - return (i, None) - else: - # Mismatched parentheses - return (-1, None) - elif char == ';': - # Found something that look like end of statements. If we are currently - # expecting a '<', the matching '>' must have been an operator, since - # template argument list should not contain statements. - while stack and stack[-1] == '>': - stack.pop() - if not stack: - return (-1, None) - - i -= 1 - - return (-1, stack) - - -def ReverseCloseExpression(clean_lines, linenum, pos): - """If input points to ) or } or ] or >, finds the position that opens it. - - If lines[linenum][pos] points to a ')' or '}' or ']' or '>', finds the - linenum/pos that correspond to the opening of the expression. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - pos: A position on the line. - - Returns: - A tuple (line, linenum, pos) pointer *at* the opening brace, or - (line, 0, -1) if we never find the matching opening brace. Note - we ignore strings and comments when matching; and the line we - return is the 'cleansed' line at linenum. - """ - line = clean_lines.elided[linenum] - if line[pos] not in ')}]>': - return (line, 0, -1) - - # Check last line - (start_pos, stack) = FindStartOfExpressionInLine(line, pos, []) - if start_pos > -1: - return (line, linenum, start_pos) - - # Continue scanning backward - while stack and linenum > 0: - linenum -= 1 - line = clean_lines.elided[linenum] - (start_pos, stack) = FindStartOfExpressionInLine(line, len(line) - 1, stack) - if start_pos > -1: - return (line, linenum, start_pos) - - # Did not find start of expression before beginning of file, give up - return (line, 0, -1) - - -def CheckForCopyright(filename, lines, error): - """Logs an error if no Copyright message appears at the top of the file.""" - - # We'll say it should occur by line 10. Don't forget there's a - # dummy line at the front. - for line in xrange(1, min(len(lines), 11)): - if re.search(r'Copyright', lines[line], re.I): break - else: # means no copyright line was found - error(filename, 0, 'legal/copyright', 5, - 'No copyright message found. ' - 'You should have a line: "Copyright [year] "') - - -def GetIndentLevel(line): - """Return the number of leading spaces in line. - - Args: - line: A string to check. - - Returns: - An integer count of leading spaces, possibly zero. - """ - indent = Match(r'^( *)\S', line) - if indent: - return len(indent.group(1)) - else: - return 0 - -def PathSplitToList(path): - """Returns the path split into a list by the separator. - - Args: - path: An absolute or relative path (e.g. '/a/b/c/' or '../a') - - Returns: - A list of path components (e.g. ['a', 'b', 'c]). - """ - lst = [] - while True: - (head, tail) = os.path.split(path) - if head == path: # absolute paths end - lst.append(head) - break - if tail == path: # relative paths end - lst.append(tail) - break - - path = head - lst.append(tail) - - lst.reverse() - return lst - -def GetHeaderGuardCPPVariable(filename): - """Returns the CPP variable that should be used as a header guard. - - Args: - filename: The name of a C++ header file. - - Returns: - The CPP variable that should be used as a header guard in the - named file. - - """ - - # Restores original filename in case that cpplint is invoked from Emacs's - # flymake. - filename = re.sub(r'_flymake\.h$', '.h', filename) - filename = re.sub(r'/\.flymake/([^/]*)$', r'/\1', filename) - # Replace 'c++' with 'cpp'. - filename = filename.replace('C++', 'cpp').replace('c++', 'cpp') - - fileinfo = FileInfo(filename) - file_path_from_root = fileinfo.RepositoryName() - - def FixupPathFromRoot(): - if _root_debug: - sys.stderr.write("\n_root fixup, _root = '%s', repository name = '%s'\n" - %(_root, fileinfo.RepositoryName())) - - # Process the file path with the --root flag if it was set. - if not _root: - if _root_debug: - sys.stderr.write("_root unspecified\n") - return file_path_from_root - - def StripListPrefix(lst, prefix): - # f(['x', 'y'], ['w, z']) -> None (not a valid prefix) - if lst[:len(prefix)] != prefix: - return None - # f(['a, 'b', 'c', 'd'], ['a', 'b']) -> ['c', 'd'] - return lst[(len(prefix)):] - - # root behavior: - # --root=subdir , lstrips subdir from the header guard - maybe_path = StripListPrefix(PathSplitToList(file_path_from_root), - PathSplitToList(_root)) - - if _root_debug: - sys.stderr.write("_root lstrip (maybe_path=%s, file_path_from_root=%s," + - " _root=%s)\n" %(maybe_path, file_path_from_root, _root)) - - if maybe_path: - return os.path.join(*maybe_path) - - # --root=.. , will prepend the outer directory to the header guard - full_path = fileinfo.FullName() - root_abspath = os.path.abspath(_root) - - maybe_path = StripListPrefix(PathSplitToList(full_path), - PathSplitToList(root_abspath)) - - if _root_debug: - sys.stderr.write("_root prepend (maybe_path=%s, full_path=%s, " + - "root_abspath=%s)\n" %(maybe_path, full_path, root_abspath)) - - if maybe_path: - return os.path.join(*maybe_path) - - if _root_debug: - sys.stderr.write("_root ignore, returning %s\n" %(file_path_from_root)) - - # --root=FAKE_DIR is ignored - return file_path_from_root - - file_path_from_root = FixupPathFromRoot() - return re.sub(r'[^a-zA-Z0-9]', '_', file_path_from_root).upper() + '_' - - -def CheckForHeaderGuard(filename, clean_lines, error): - """Checks that the file contains a header guard. - - Logs an error if no #ifndef header guard is present. For other - headers, checks that the full pathname is used. - - Args: - filename: The name of the C++ header file. - clean_lines: A CleansedLines instance containing the file. - error: The function to call with any errors found. - """ - - # Don't check for header guards if there are error suppression - # comments somewhere in this file. - # - # Because this is silencing a warning for a nonexistent line, we - # only support the very specific NOLINT(build/header_guard) syntax, - # and not the general NOLINT or NOLINT(*) syntax. - raw_lines = clean_lines.lines_without_raw_strings - for i in raw_lines: - if Search(r'//\s*NOLINT\(build/header_guard\)', i): - return - - cppvar = GetHeaderGuardCPPVariable(filename) - - ifndef = '' - ifndef_linenum = 0 - define = '' - endif = '' - endif_linenum = 0 - for linenum, line in enumerate(raw_lines): - linesplit = line.split() - if len(linesplit) >= 2: - # find the first occurrence of #ifndef and #define, save arg - if not ifndef and linesplit[0] == '#ifndef': - # set ifndef to the header guard presented on the #ifndef line. - ifndef = linesplit[1] - ifndef_linenum = linenum - if not define and linesplit[0] == '#define': - define = linesplit[1] - # find the last occurrence of #endif, save entire line - if line.startswith('#endif'): - endif = line - endif_linenum = linenum - - if not ifndef or not define or ifndef != define: - error(filename, 0, 'build/header_guard', 5, - 'No #ifndef header guard found, suggested CPP variable is: %s' % - cppvar) - return - - # The guard should be PATH_FILE_H_, but we also allow PATH_FILE_H__ - # for backward compatibility. - if ifndef != cppvar: - error_level = 0 - if ifndef != cppvar + '_': - error_level = 5 - - ParseNolintSuppressions(filename, raw_lines[ifndef_linenum], ifndef_linenum, - error) - error(filename, ifndef_linenum, 'build/header_guard', error_level, - '#ifndef header guard has wrong style, please use: %s' % cppvar) - - # Check for "//" comments on endif line. - # ParseNolintSuppressions(filename, raw_lines[endif_linenum], endif_linenum, - # error) - # match = Match(r'#endif\s*//\s*' + cppvar + r'(_)?\b', endif) - # if match: - # if match.group(1) == '_': - # # Issue low severity warning for deprecated double trailing underscore - # error(filename, endif_linenum, 'build/header_guard', 0, - # '#endif line should be "#endif // %s"' % cppvar) - # return - - # Didn't find the corresponding "//" comment. If this file does not - # contain any "//" comments at all, it could be that the compiler - # only wants "/**/" comments, look for those instead. - # no_single_line_comments = True - # for i in xrange(1, len(raw_lines) - 1): - # line = raw_lines[i] - # if Match(r'^(?:(?:\'(?:\.|[^\'])*\')|(?:"(?:\.|[^"])*")|[^\'"])*//', line): - # no_single_line_comments = False - # break - - # if no_single_line_comments: - # match = Match(r'#endif\s*/\*\s*' + cppvar + r'(_)?\s*\*/', endif) - # if match: - # if match.group(1) == '_': - # # Low severity warning for double trailing underscore - # error(filename, endif_linenum, 'build/header_guard', 0, - # '#endif line should be "#endif /* %s */"' % cppvar) - # return - - # # Didn't find anything - # error(filename, endif_linenum, 'build/header_guard', 5, - # '#endif line should be "#endif // %s"' % cppvar) - - -def CheckHeaderFileIncluded(filename, include_state, error): - """Logs an error if a .cc file does not include its header.""" - - # Do not check test files - fileinfo = FileInfo(filename) - if Search(_TEST_FILE_SUFFIX, fileinfo.BaseName()): - return - - headerfile = filename[0:len(filename) - len(fileinfo.Extension())] + '.h' - if not os.path.exists(headerfile): - return - headername = FileInfo(headerfile).RepositoryName() - first_include = 0 - for section_list in include_state.include_list: - for f in section_list: - if headername in f[0] or f[0] in headername: - return - if not first_include: - first_include = f[1] - - error(filename, first_include, 'build/include', 5, - '%s should include its header file %s' % (fileinfo.RepositoryName(), - headername)) - - -def CheckForBadCharacters(filename, lines, error): - """Logs an error for each line containing bad characters. - - Two kinds of bad characters: - - 1. Unicode replacement characters: These indicate that either the file - contained invalid UTF-8 (likely) or Unicode replacement characters (which - it shouldn't). Note that it's possible for this to throw off line - numbering if the invalid UTF-8 occurred adjacent to a newline. - - 2. NUL bytes. These are problematic for some tools. - - Args: - filename: The name of the current file. - lines: An array of strings, each representing a line of the file. - error: The function to call with any errors found. - """ - for linenum, line in enumerate(lines): - if u'\ufffd' in line: - error(filename, linenum, 'readability/utf8', 5, - 'Line contains invalid UTF-8 (or Unicode replacement character).') - if '\0' in line: - error(filename, linenum, 'readability/nul', 5, 'Line contains NUL byte.') - - -def CheckForNewlineAtEOF(filename, lines, error): - """Logs an error if there is no newline char at the end of the file. - - Args: - filename: The name of the current file. - lines: An array of strings, each representing a line of the file. - error: The function to call with any errors found. - """ - - # The array lines() was created by adding two newlines to the - # original file (go figure), then splitting on \n. - # To verify that the file ends in \n, we just have to make sure the - # last-but-two element of lines() exists and is empty. - if len(lines) < 3 or lines[-2]: - error(filename, len(lines) - 2, 'whitespace/ending_newline', 5, - 'Could not find a newline character at the end of the file.') - - -def CheckForMultilineCommentsAndStrings(filename, clean_lines, linenum, error): - """Logs an error if we see /* ... */ or "..." that extend past one line. - - /* ... */ comments are legit inside macros, for one line. - Otherwise, we prefer // comments, so it's ok to warn about the - other. Likewise, it's ok for strings to extend across multiple - lines, as long as a line continuation character (backslash) - terminates each line. Although not currently prohibited by the C++ - style guide, it's ugly and unnecessary. We don't do well with either - in this lint program, so we warn about both. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Remove all \\ (escaped backslashes) from the line. They are OK, and the - # second (escaped) slash may trigger later \" detection erroneously. - line = line.replace('\\\\', '') - - if line.count('/*') > line.count('*/'): - error(filename, linenum, 'readability/multiline_comment', 5, - 'Complex multi-line /*...*/-style comment found. ' - 'Lint may give bogus warnings. ' - 'Consider replacing these with //-style comments, ' - 'with #if 0...#endif, ' - 'or with more clearly structured multi-line comments.') - - if (line.count('"') - line.count('\\"')) % 2: - error(filename, linenum, 'readability/multiline_string', 5, - 'Multi-line string ("...") found. This lint script doesn\'t ' - 'do well with such strings, and may give bogus warnings. ' - 'Use C++11 raw strings or concatenation instead.') - - -# (non-threadsafe name, thread-safe alternative, validation pattern) -# -# The validation pattern is used to eliminate false positives such as: -# _rand(); // false positive due to substring match. -# ->rand(); // some member function rand(). -# ACMRandom rand(seed); // some variable named rand. -# ISAACRandom rand(); // another variable named rand. -# -# Basically we require the return value of these functions to be used -# in some expression context on the same line by matching on some -# operator before the function name. This eliminates constructors and -# member function calls. -_UNSAFE_FUNC_PREFIX = r'(?:[-+*/=%^&|(<]\s*|>\s+)' -_THREADING_LIST = ( - ('asctime(', 'asctime_r(', _UNSAFE_FUNC_PREFIX + r'asctime\([^)]+\)'), - ('ctime(', 'ctime_r(', _UNSAFE_FUNC_PREFIX + r'ctime\([^)]+\)'), - ('getgrgid(', 'getgrgid_r(', _UNSAFE_FUNC_PREFIX + r'getgrgid\([^)]+\)'), - ('getgrnam(', 'getgrnam_r(', _UNSAFE_FUNC_PREFIX + r'getgrnam\([^)]+\)'), - ('getlogin(', 'getlogin_r(', _UNSAFE_FUNC_PREFIX + r'getlogin\(\)'), - ('getpwnam(', 'getpwnam_r(', _UNSAFE_FUNC_PREFIX + r'getpwnam\([^)]+\)'), - ('getpwuid(', 'getpwuid_r(', _UNSAFE_FUNC_PREFIX + r'getpwuid\([^)]+\)'), - ('gmtime(', 'gmtime_r(', _UNSAFE_FUNC_PREFIX + r'gmtime\([^)]+\)'), - ('localtime(', 'localtime_r(', _UNSAFE_FUNC_PREFIX + r'localtime\([^)]+\)'), - ('rand(', 'rand_r(', _UNSAFE_FUNC_PREFIX + r'rand\(\)'), - ('strtok(', 'strtok_r(', - _UNSAFE_FUNC_PREFIX + r'strtok\([^)]+\)'), - ('ttyname(', 'ttyname_r(', _UNSAFE_FUNC_PREFIX + r'ttyname\([^)]+\)'), - ) - - -def CheckPosixThreading(filename, clean_lines, linenum, error): - """Checks for calls to thread-unsafe functions. - - Much code has been originally written without consideration of - multi-threading. Also, engineers are relying on their old experience; - they have learned posix before threading extensions were added. These - tests guide the engineers to use thread-safe functions (when using - posix directly). - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - for single_thread_func, multithread_safe_func, pattern in _THREADING_LIST: - # Additional pattern matching check to confirm that this is the - # function we are looking for - if Search(pattern, line): - error(filename, linenum, 'runtime/threadsafe_fn', 2, - 'Consider using ' + multithread_safe_func + - '...) instead of ' + single_thread_func + - '...) for improved thread safety.') - - -def CheckVlogArguments(filename, clean_lines, linenum, error): - """Checks that VLOG() is only used for defining a logging level. - - For example, VLOG(2) is correct. VLOG(INFO), VLOG(WARNING), VLOG(ERROR), and - VLOG(FATAL) are not. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - if Search(r'\bVLOG\((INFO|ERROR|WARNING|DFATAL|FATAL)\)', line): - error(filename, linenum, 'runtime/vlog', 5, - 'VLOG() should be used with numeric verbosity level. ' - 'Use LOG() if you want symbolic severity levels.') - -# Matches invalid increment: *count++, which moves pointer instead of -# incrementing a value. -_RE_PATTERN_INVALID_INCREMENT = re.compile( - r'^\s*\*\w+(\+\+|--);') - - -def CheckInvalidIncrement(filename, clean_lines, linenum, error): - """Checks for invalid increment *count++. - - For example following function: - void increment_counter(int* count) { - *count++; - } - is invalid, because it effectively does count++, moving pointer, and should - be replaced with ++*count, (*count)++ or *count += 1. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - if _RE_PATTERN_INVALID_INCREMENT.match(line): - error(filename, linenum, 'runtime/invalid_increment', 5, - 'Changing pointer instead of value (or unused value of operator*).') - - -def IsMacroDefinition(clean_lines, linenum): - if Search(r'^#define', clean_lines[linenum]): - return True - - if linenum > 0 and Search(r'\\$', clean_lines[linenum - 1]): - return True - - return False - - -def IsForwardClassDeclaration(clean_lines, linenum): - return Match(r'^\s*(\btemplate\b)*.*class\s+\w+;\s*$', clean_lines[linenum]) - - -class _BlockInfo(object): - """Stores information about a generic block of code.""" - - def __init__(self, linenum, seen_open_brace): - self.starting_linenum = linenum - self.seen_open_brace = seen_open_brace - self.open_parentheses = 0 - self.inline_asm = _NO_ASM - self.check_namespace_indentation = False - - def CheckBegin(self, filename, clean_lines, linenum, error): - """Run checks that applies to text up to the opening brace. - - This is mostly for checking the text after the class identifier - and the "{", usually where the base class is specified. For other - blocks, there isn't much to check, so we always pass. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - pass - - def CheckEnd(self, filename, clean_lines, linenum, error): - """Run checks that applies to text after the closing brace. - - This is mostly used for checking end of namespace comments. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - pass - - def IsBlockInfo(self): - """Returns true if this block is a _BlockInfo. - - This is convenient for verifying that an object is an instance of - a _BlockInfo, but not an instance of any of the derived classes. - - Returns: - True for this class, False for derived classes. - """ - return self.__class__ == _BlockInfo - - -class _ExternCInfo(_BlockInfo): - """Stores information about an 'extern "C"' block.""" - - def __init__(self, linenum): - _BlockInfo.__init__(self, linenum, True) - - -class _ClassInfo(_BlockInfo): - """Stores information about a class.""" - - def __init__(self, name, class_or_struct, clean_lines, linenum): - _BlockInfo.__init__(self, linenum, False) - self.name = name - self.is_derived = False - self.check_namespace_indentation = True - if class_or_struct == 'struct': - self.access = 'public' - self.is_struct = True - else: - self.access = 'private' - self.is_struct = False - - # Remember initial indentation level for this class. Using raw_lines here - # instead of elided to account for leading comments. - self.class_indent = GetIndentLevel(clean_lines.raw_lines[linenum]) - - # Try to find the end of the class. This will be confused by things like: - # class A { - # } *x = { ... - # - # But it's still good enough for CheckSectionSpacing. - self.last_line = 0 - depth = 0 - for i in range(linenum, clean_lines.NumLines()): - line = clean_lines.elided[i] - depth += line.count('{') - line.count('}') - if not depth: - self.last_line = i - break - - def CheckBegin(self, filename, clean_lines, linenum, error): - # Look for a bare ':' - if Search('(^|[^:]):($|[^:])', clean_lines.elided[linenum]): - self.is_derived = True - - def CheckEnd(self, filename, clean_lines, linenum, error): - # If there is a DISALLOW macro, it should appear near the end of - # the class. - seen_last_thing_in_class = False - for i in xrange(linenum - 1, self.starting_linenum, -1): - match = Search( - r'\b(DISALLOW_COPY_AND_ASSIGN|DISALLOW_IMPLICIT_CONSTRUCTORS)\(' + - self.name + r'\)', - clean_lines.elided[i]) - if match: - if seen_last_thing_in_class: - error(filename, i, 'readability/constructors', 3, - match.group(1) + ' should be the last thing in the class') - break - - if not Match(r'^\s*$', clean_lines.elided[i]): - seen_last_thing_in_class = True - - # Check that closing brace is aligned with beginning of the class. - # Only do this if the closing brace is indented by only whitespaces. - # This means we will not check single-line class definitions. - indent = Match(r'^( *)\}', clean_lines.elided[linenum]) - if indent and len(indent.group(1)) != self.class_indent: - if self.is_struct: - parent = 'struct ' + self.name - else: - parent = 'class ' + self.name - error(filename, linenum, 'whitespace/indent', 3, - 'Closing brace should be aligned with beginning of %s' % parent) - - -class _NamespaceInfo(_BlockInfo): - """Stores information about a namespace.""" - - def __init__(self, name, linenum): - _BlockInfo.__init__(self, linenum, False) - self.name = name or '' - self.check_namespace_indentation = True - - def CheckEnd(self, filename, clean_lines, linenum, error): - """Check end of namespace comments.""" - line = clean_lines.raw_lines[linenum] - - # Check how many lines is enclosed in this namespace. Don't issue - # warning for missing namespace comments if there aren't enough - # lines. However, do apply checks if there is already an end of - # namespace comment and it's incorrect. - # - # TODO(unknown): We always want to check end of namespace comments - # if a namespace is large, but sometimes we also want to apply the - # check if a short namespace contained nontrivial things (something - # other than forward declarations). There is currently no logic on - # deciding what these nontrivial things are, so this check is - # triggered by namespace size only, which works most of the time. - if (linenum - self.starting_linenum < 10 - and not Match(r'^\s*};*\s*(//|/\*).*\bnamespace\b', line)): - return - - # Look for matching comment at end of namespace. - # - # Note that we accept C style "/* */" comments for terminating - # namespaces, so that code that terminate namespaces inside - # preprocessor macros can be cpplint clean. - # - # We also accept stuff like "// end of namespace ." with the - # period at the end. - # - # Besides these, we don't accept anything else, otherwise we might - # get false negatives when existing comment is a substring of the - # expected namespace. - if self.name: - return - else: - # Anonymous namespace - if not Match(r'^\s*};*\s*(//|/\*).*\bnamespace[\*/\.\\\s]*$', line): - # If "// namespace anonymous" or "// anonymous namespace (more text)", - # mention "// anonymous namespace" as an acceptable form - if Match(r'^\s*}.*\b(namespace anonymous|anonymous namespace)\b', line): - error(filename, linenum, 'readability/namespace', 5, - 'Anonymous namespace should be terminated with "// namespace"' - ' or "// anonymous namespace"') - else: - error(filename, linenum, 'readability/namespace', 5, - 'Anonymous namespace should be terminated with "// namespace"') - - -class _PreprocessorInfo(object): - """Stores checkpoints of nesting stacks when #if/#else is seen.""" - - def __init__(self, stack_before_if): - # The entire nesting stack before #if - self.stack_before_if = stack_before_if - - # The entire nesting stack up to #else - self.stack_before_else = [] - - # Whether we have already seen #else or #elif - self.seen_else = False - - -class NestingState(object): - """Holds states related to parsing braces.""" - - def __init__(self): - # Stack for tracking all braces. An object is pushed whenever we - # see a "{", and popped when we see a "}". Only 3 types of - # objects are possible: - # - _ClassInfo: a class or struct. - # - _NamespaceInfo: a namespace. - # - _BlockInfo: some other type of block. - self.stack = [] - - # Top of the previous stack before each Update(). - # - # Because the nesting_stack is updated at the end of each line, we - # had to do some convoluted checks to find out what is the current - # scope at the beginning of the line. This check is simplified by - # saving the previous top of nesting stack. - # - # We could save the full stack, but we only need the top. Copying - # the full nesting stack would slow down cpplint by ~10%. - self.previous_stack_top = [] - - # Stack of _PreprocessorInfo objects. - self.pp_stack = [] - - def SeenOpenBrace(self): - """Check if we have seen the opening brace for the innermost block. - - Returns: - True if we have seen the opening brace, False if the innermost - block is still expecting an opening brace. - """ - return (not self.stack) or self.stack[-1].seen_open_brace - - def InNamespaceBody(self): - """Check if we are currently one level inside a namespace body. - - Returns: - True if top of the stack is a namespace block, False otherwise. - """ - return self.stack and isinstance(self.stack[-1], _NamespaceInfo) - - def InExternC(self): - """Check if we are currently one level inside an 'extern "C"' block. - - Returns: - True if top of the stack is an extern block, False otherwise. - """ - return self.stack and isinstance(self.stack[-1], _ExternCInfo) - - def InClassDeclaration(self): - """Check if we are currently one level inside a class or struct declaration. - - Returns: - True if top of the stack is a class/struct, False otherwise. - """ - return self.stack and isinstance(self.stack[-1], _ClassInfo) - - def InAsmBlock(self): - """Check if we are currently one level inside an inline ASM block. - - Returns: - True if the top of the stack is a block containing inline ASM. - """ - return self.stack and self.stack[-1].inline_asm != _NO_ASM - - def InTemplateArgumentList(self, clean_lines, linenum, pos): - """Check if current position is inside template argument list. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - pos: position just after the suspected template argument. - Returns: - True if (linenum, pos) is inside template arguments. - """ - while linenum < clean_lines.NumLines(): - # Find the earliest character that might indicate a template argument - line = clean_lines.elided[linenum] - match = Match(r'^[^{};=\[\]\.<>]*(.)', line[pos:]) - if not match: - linenum += 1 - pos = 0 - continue - token = match.group(1) - pos += len(match.group(0)) - - # These things do not look like template argument list: - # class Suspect { - # class Suspect x; } - if token in ('{', '}', ';'): return False - - # These things look like template argument list: - # template - # template - # template - # template - if token in ('>', '=', '[', ']', '.'): return True - - # Check if token is an unmatched '<'. - # If not, move on to the next character. - if token != '<': - pos += 1 - if pos >= len(line): - linenum += 1 - pos = 0 - continue - - # We can't be sure if we just find a single '<', and need to - # find the matching '>'. - (_, end_line, end_pos) = CloseExpression(clean_lines, linenum, pos - 1) - if end_pos < 0: - # Not sure if template argument list or syntax error in file - return False - linenum = end_line - pos = end_pos - return False - - def UpdatePreprocessor(self, line): - """Update preprocessor stack. - - We need to handle preprocessors due to classes like this: - #ifdef SWIG - struct ResultDetailsPageElementExtensionPoint { - #else - struct ResultDetailsPageElementExtensionPoint : public Extension { - #endif - - We make the following assumptions (good enough for most files): - - Preprocessor condition evaluates to true from #if up to first - #else/#elif/#endif. - - - Preprocessor condition evaluates to false from #else/#elif up - to #endif. We still perform lint checks on these lines, but - these do not affect nesting stack. - - Args: - line: current line to check. - """ - if Match(r'^\s*#\s*(if|ifdef|ifndef)\b', line): - # Beginning of #if block, save the nesting stack here. The saved - # stack will allow us to restore the parsing state in the #else case. - self.pp_stack.append(_PreprocessorInfo(copy.deepcopy(self.stack))) - elif Match(r'^\s*#\s*(else|elif)\b', line): - # Beginning of #else block - if self.pp_stack: - if not self.pp_stack[-1].seen_else: - # This is the first #else or #elif block. Remember the - # whole nesting stack up to this point. This is what we - # keep after the #endif. - self.pp_stack[-1].seen_else = True - self.pp_stack[-1].stack_before_else = copy.deepcopy(self.stack) - - # Restore the stack to how it was before the #if - self.stack = copy.deepcopy(self.pp_stack[-1].stack_before_if) - else: - # TODO(unknown): unexpected #else, issue warning? - pass - elif Match(r'^\s*#\s*endif\b', line): - # End of #if or #else blocks. - if self.pp_stack: - # If we saw an #else, we will need to restore the nesting - # stack to its former state before the #else, otherwise we - # will just continue from where we left off. - if self.pp_stack[-1].seen_else: - # Here we can just use a shallow copy since we are the last - # reference to it. - self.stack = self.pp_stack[-1].stack_before_else - # Drop the corresponding #if - self.pp_stack.pop() - else: - # TODO(unknown): unexpected #endif, issue warning? - pass - - # TODO(unknown): Update() is too long, but we will refactor later. - def Update(self, filename, clean_lines, linenum, error): - """Update nesting state with current line. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Remember top of the previous nesting stack. - # - # The stack is always pushed/popped and not modified in place, so - # we can just do a shallow copy instead of copy.deepcopy. Using - # deepcopy would slow down cpplint by ~28%. - if self.stack: - self.previous_stack_top = self.stack[-1] - else: - self.previous_stack_top = None - - # Update pp_stack - self.UpdatePreprocessor(line) - - # Count parentheses. This is to avoid adding struct arguments to - # the nesting stack. - if self.stack: - inner_block = self.stack[-1] - depth_change = line.count('(') - line.count(')') - inner_block.open_parentheses += depth_change - - # Also check if we are starting or ending an inline assembly block. - if inner_block.inline_asm in (_NO_ASM, _END_ASM): - if (depth_change != 0 and - inner_block.open_parentheses == 1 and - _MATCH_ASM.match(line)): - # Enter assembly block - inner_block.inline_asm = _INSIDE_ASM - else: - # Not entering assembly block. If previous line was _END_ASM, - # we will now shift to _NO_ASM state. - inner_block.inline_asm = _NO_ASM - elif (inner_block.inline_asm == _INSIDE_ASM and - inner_block.open_parentheses == 0): - # Exit assembly block - inner_block.inline_asm = _END_ASM - - # Consume namespace declaration at the beginning of the line. Do - # this in a loop so that we catch same line declarations like this: - # namespace proto2 { namespace bridge { class MessageSet; } } - while True: - # Match start of namespace. The "\b\s*" below catches namespace - # declarations even if it weren't followed by a whitespace, this - # is so that we don't confuse our namespace checker. The - # missing spaces will be flagged by CheckSpacing. - namespace_decl_match = Match(r'^\s*namespace\b\s*([:\w]+)?(.*)$', line) - if not namespace_decl_match: - break - - new_namespace = _NamespaceInfo(namespace_decl_match.group(1), linenum) - self.stack.append(new_namespace) - - line = namespace_decl_match.group(2) - if line.find('{') != -1: - new_namespace.seen_open_brace = True - line = line[line.find('{') + 1:] - - # Look for a class declaration in whatever is left of the line - # after parsing namespaces. The regexp accounts for decorated classes - # such as in: - # class LOCKABLE API Object { - # }; - class_decl_match = Match( - r'^(\s*(?:template\s*<[\w\s<>,:]*>\s*)?' - r'(class|struct)\s+(?:[A-Z_]+\s+)*(\w+(?:::\w+)*))' - r'(.*)$', line) - if (class_decl_match and - (not self.stack or self.stack[-1].open_parentheses == 0)): - # We do not want to accept classes that are actually template arguments: - # template , - # template class Ignore3> - # void Function() {}; - # - # To avoid template argument cases, we scan forward and look for - # an unmatched '>'. If we see one, assume we are inside a - # template argument list. - end_declaration = len(class_decl_match.group(1)) - if not self.InTemplateArgumentList(clean_lines, linenum, end_declaration): - self.stack.append(_ClassInfo( - class_decl_match.group(3), class_decl_match.group(2), - clean_lines, linenum)) - line = class_decl_match.group(4) - - # If we have not yet seen the opening brace for the innermost block, - # run checks here. - if not self.SeenOpenBrace(): - self.stack[-1].CheckBegin(filename, clean_lines, linenum, error) - - # Update access control if we are inside a class/struct - if self.stack and isinstance(self.stack[-1], _ClassInfo): - classinfo = self.stack[-1] - access_match = Match( - r'^(.*)\b(public|private|protected|signals)(\s+(?:slots\s*)?)?' - r':(?:[^:]|$)', - line) - if access_match: - classinfo.access = access_match.group(2) - - # Check that access keywords are indented +2 space. Skip this - # check if the keywords are not preceded by whitespaces. - indent = access_match.group(1) - if (len(indent) != classinfo.class_indent + 2 and - Match(r'^\s*$', indent)): - if classinfo.is_struct: - parent = 'struct ' + classinfo.name - else: - parent = 'class ' + classinfo.name - slots = '' - if access_match.group(3): - slots = access_match.group(3) - error(filename, linenum, 'whitespace/indent', 3, - '%s%s: should be indented +1 space inside %s' % ( - access_match.group(2), slots, parent)) - - # Consume braces or semicolons from what's left of the line - while True: - # Match first brace, semicolon, or closed parenthesis. - matched = Match(r'^[^{;)}]*([{;)}])(.*)$', line) - if not matched: - break - - token = matched.group(1) - if token == '{': - # If namespace or class hasn't seen a opening brace yet, mark - # namespace/class head as complete. Push a new block onto the - # stack otherwise. - if not self.SeenOpenBrace(): - self.stack[-1].seen_open_brace = True - elif Match(r'^extern\s*"[^"]*"\s*\{', line): - self.stack.append(_ExternCInfo(linenum)) - else: - self.stack.append(_BlockInfo(linenum, True)) - if _MATCH_ASM.match(line): - self.stack[-1].inline_asm = _BLOCK_ASM - - elif token == ';' or token == ')': - # If we haven't seen an opening brace yet, but we already saw - # a semicolon, this is probably a forward declaration. Pop - # the stack for these. - # - # Similarly, if we haven't seen an opening brace yet, but we - # already saw a closing parenthesis, then these are probably - # function arguments with extra "class" or "struct" keywords. - # Also pop these stack for these. - if not self.SeenOpenBrace(): - self.stack.pop() - else: # token == '}' - # Perform end of block checks and pop the stack. - if self.stack: - self.stack[-1].CheckEnd(filename, clean_lines, linenum, error) - self.stack.pop() - line = matched.group(2) - - def InnermostClass(self): - """Get class info on the top of the stack. - - Returns: - A _ClassInfo object if we are inside a class, or None otherwise. - """ - for i in range(len(self.stack), 0, -1): - classinfo = self.stack[i - 1] - if isinstance(classinfo, _ClassInfo): - return classinfo - return None - - def CheckCompletedBlocks(self, filename, error): - """Checks that all classes and namespaces have been completely parsed. - - Call this when all lines in a file have been processed. - Args: - filename: The name of the current file. - error: The function to call with any errors found. - """ - # Note: This test can result in false positives if #ifdef constructs - # get in the way of brace matching. See the testBuildClass test in - # cpplint_unittest.py for an example of this. - for obj in self.stack: - if isinstance(obj, _ClassInfo): - error(filename, obj.starting_linenum, 'build/class', 5, - 'Failed to find complete declaration of class %s' % - obj.name) - elif isinstance(obj, _NamespaceInfo): - error(filename, obj.starting_linenum, 'build/namespaces', 5, - 'Failed to find complete declaration of namespace %s' % - obj.name) - - -def CheckForNonStandardConstructs(filename, clean_lines, linenum, - nesting_state, error): - r"""Logs an error if we see certain non-ANSI constructs ignored by gcc-2. - - Complain about several constructs which gcc-2 accepts, but which are - not standard C++. Warning about these in lint is one way to ease the - transition to new compilers. - - put storage class first (e.g. "static const" instead of "const static"). - - "%lld" instead of %qd" in printf-type functions. - - "%1$d" is non-standard in printf-type functions. - - "\%" is an undefined character escape sequence. - - text after #endif is not allowed. - - invalid inner-style forward declaration. - - >? and ?= and )\?=?\s*(\w+|[+-]?\d+)(\.\d*)?', - line): - error(filename, linenum, 'build/deprecated', 3, - '>? and ))?' - # r'\s*const\s*' + type_name + '\s*&\s*\w+\s*;' - error(filename, linenum, 'runtime/member_string_references', 2, - 'const string& members are dangerous. It is much better to use ' - 'alternatives, such as pointers or simple constants.') - - # Everything else in this function operates on class declarations. - # Return early if the top of the nesting stack is not a class, or if - # the class head is not completed yet. - classinfo = nesting_state.InnermostClass() - if not classinfo or not classinfo.seen_open_brace: - return - - # The class may have been declared with namespace or classname qualifiers. - # The constructor and destructor will not have those qualifiers. - base_classname = classinfo.name.split('::')[-1] - - # Look for single-argument constructors that aren't marked explicit. - # Technically a valid construct, but against style. - explicit_constructor_match = Match( - r'\s+(?:(?:inline|constexpr)\s+)*(explicit\s+)?' - r'(?:(?:inline|constexpr)\s+)*%s\s*' - r'\(((?:[^()]|\([^()]*\))*)\)' - % re.escape(base_classname), - line) - - if explicit_constructor_match: - is_marked_explicit = explicit_constructor_match.group(1) - - if not explicit_constructor_match.group(2): - constructor_args = [] - else: - constructor_args = explicit_constructor_match.group(2).split(',') - - # collapse arguments so that commas in template parameter lists and function - # argument parameter lists don't split arguments in two - i = 0 - while i < len(constructor_args): - constructor_arg = constructor_args[i] - while (constructor_arg.count('<') > constructor_arg.count('>') or - constructor_arg.count('(') > constructor_arg.count(')')): - constructor_arg += ',' + constructor_args[i + 1] - del constructor_args[i + 1] - constructor_args[i] = constructor_arg - i += 1 - - defaulted_args = [arg for arg in constructor_args if '=' in arg] - noarg_constructor = (not constructor_args or # empty arg list - # 'void' arg specifier - (len(constructor_args) == 1 and - constructor_args[0].strip() == 'void')) - onearg_constructor = ((len(constructor_args) == 1 and # exactly one arg - not noarg_constructor) or - # all but at most one arg defaulted - (len(constructor_args) >= 1 and - not noarg_constructor and - len(defaulted_args) >= len(constructor_args) - 1)) - initializer_list_constructor = bool( - onearg_constructor and - Search(r'\bstd\s*::\s*initializer_list\b', constructor_args[0])) - copy_constructor = bool( - onearg_constructor and - Match(r'(const\s+)?%s(\s*<[^>]*>)?(\s+const)?\s*(?:<\w+>\s*)?&' - % re.escape(base_classname), constructor_args[0].strip())) - - if (not is_marked_explicit and - onearg_constructor and - not initializer_list_constructor and - not copy_constructor): - if defaulted_args: - error(filename, linenum, 'runtime/explicit', 5, - 'Constructors callable with one argument ' - 'should be marked explicit.') - else: - error(filename, linenum, 'runtime/explicit', 5, - 'Single-parameter constructors should be marked explicit.') - elif is_marked_explicit and not onearg_constructor: - if noarg_constructor: - error(filename, linenum, 'runtime/explicit', 5, - 'Zero-parameter constructors should not be marked explicit.') - - -def CheckSpacingForFunctionCall(filename, clean_lines, linenum, error): - """Checks for the correctness of various spacing around function calls. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Since function calls often occur inside if/for/while/switch - # expressions - which have their own, more liberal conventions - we - # first see if we should be looking inside such an expression for a - # function call, to which we can apply more strict standards. - fncall = line # if there's no control flow construct, look at whole line - for pattern in (r'\bif\s*\((.*)\)\s*{', - r'\bfor\s*\((.*)\)\s*{', - r'\bwhile\s*\((.*)\)\s*[{;]', - r'\bswitch\s*\((.*)\)\s*{'): - match = Search(pattern, line) - if match: - fncall = match.group(1) # look inside the parens for function calls - break - - # Except in if/for/while/switch, there should never be space - # immediately inside parens (eg "f( 3, 4 )"). We make an exception - # for nested parens ( (a+b) + c ). Likewise, there should never be - # a space before a ( when it's a function argument. I assume it's a - # function argument when the char before the whitespace is legal in - # a function name (alnum + _) and we're not starting a macro. Also ignore - # pointers and references to arrays and functions coz they're too tricky: - # we use a very simple way to recognize these: - # " (something)(maybe-something)" or - # " (something)(maybe-something," or - # " (something)[something]" - # Note that we assume the contents of [] to be short enough that - # they'll never need to wrap. - if ( # Ignore control structures. - not Search(r'\b(if|for|while|switch|return|new|delete|catch|sizeof)\b', - fncall) and - # Ignore pointers/references to functions. - not Search(r' \([^)]+\)\([^)]*(\)|,$)', fncall) and - # Ignore pointers/references to arrays. - not Search(r' \([^)]+\)\[[^\]]+\]', fncall)): - if Search(r'\w\s*\(\s(?!\s*\\$)', fncall): # a ( used for a fn call - error(filename, linenum, 'whitespace/parens', 4, - 'Extra space after ( in function call') - elif Search(r'\(\s+(?!(\s*\\)|\()', fncall): - error(filename, linenum, 'whitespace/parens', 2, - 'Extra space after (') - if (Search(r'\w\s+\(', fncall) and - not Search(r'_{0,2}asm_{0,2}\s+_{0,2}volatile_{0,2}\s+\(', fncall) and - not Search(r'#\s*define|typedef|using\s+\w+\s*=', fncall) and - not Search(r'\w\s+\((\w+::)*\*\w+\)\(', fncall) and - not Search(r'\bcase\s+\(', fncall)): - # TODO(unknown): Space after an operator function seem to be a common - # error, silence those for now by restricting them to highest verbosity. - if Search(r'\boperator_*\b', line): - error(filename, linenum, 'whitespace/parens', 0, - 'Extra space before ( in function call') - else: - error(filename, linenum, 'whitespace/parens', 4, - 'Extra space before ( in function call') - # If the ) is followed only by a newline or a { + newline, assume it's - # part of a control statement (if/while/etc), and don't complain - if Search(r'[^)]\s+\)\s*[^{\s]', fncall): - # If the closing parenthesis is preceded by only whitespaces, - # try to give a more descriptive error message. - if Search(r'^\s+\)', fncall): - error(filename, linenum, 'whitespace/parens', 2, - 'Closing ) should be moved to the previous line') - else: - error(filename, linenum, 'whitespace/parens', 2, - 'Extra space before )') - - -def IsBlankLine(line): - """Returns true if the given line is blank. - - We consider a line to be blank if the line is empty or consists of - only white spaces. - - Args: - line: A line of a string. - - Returns: - True, if the given line is blank. - """ - return not line or line.isspace() - - -def CheckForNamespaceIndentation(filename, nesting_state, clean_lines, line, - error): - is_namespace_indent_item = ( - len(nesting_state.stack) > 1 and - nesting_state.stack[-1].check_namespace_indentation and - isinstance(nesting_state.previous_stack_top, _NamespaceInfo) and - nesting_state.previous_stack_top == nesting_state.stack[-2]) - - if ShouldCheckNamespaceIndentation(nesting_state, is_namespace_indent_item, - clean_lines.elided, line): - CheckItemIndentationInNamespace(filename, clean_lines.elided, - line, error) - - -def CheckForFunctionLengths(filename, clean_lines, linenum, - function_state, error): - """Reports for long function bodies. - - For an overview why this is done, see: - https://google-styleguide.googlecode.com/svn/trunk/cppguide.xml#Write_Short_Functions - - Uses a simplistic algorithm assuming other style guidelines - (especially spacing) are followed. - Only checks unindented functions, so class members are unchecked. - Trivial bodies are unchecked, so constructors with huge initializer lists - may be missed. - Blank/comment lines are not counted so as to avoid encouraging the removal - of vertical space and comments just to get through a lint check. - NOLINT *on the last line of a function* disables this check. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - function_state: Current function name and lines in body so far. - error: The function to call with any errors found. - """ - lines = clean_lines.lines - line = lines[linenum] - joined_line = '' - - starting_func = False - regexp = r'(\w(\w|::|\*|\&|\s)*)\(' # decls * & space::name( ... - match_result = Match(regexp, line) - if match_result: - # If the name is all caps and underscores, figure it's a macro and - # ignore it, unless it's TEST or TEST_F. - function_name = match_result.group(1).split()[-1] - if function_name == 'TEST' or function_name == 'TEST_F' or ( - not Match(r'[A-Z_]+$', function_name)): - starting_func = True - - if starting_func: - body_found = False - for start_linenum in xrange(linenum, clean_lines.NumLines()): - start_line = lines[start_linenum] - joined_line += ' ' + start_line.lstrip() - if Search(r'(;|})', start_line): # Declarations and trivial functions - body_found = True - break # ... ignore - elif Search(r'{', start_line): - body_found = True - function = Search(r'((\w|:)*)\(', line).group(1) - if Match(r'TEST', function): # Handle TEST... macros - parameter_regexp = Search(r'(\(.*\))', joined_line) - if parameter_regexp: # Ignore bad syntax - function += parameter_regexp.group(1) - else: - function += '()' - function_state.Begin(function) - break - if not body_found: - # No body for the function (or evidence of a non-function) was found. - error(filename, linenum, 'readability/fn_size', 5, - 'Lint failed to find start of function body.') - elif Match(r'^\}\s*$', line): # function end - function_state.Check(error, filename, linenum) - function_state.End() - elif not Match(r'^\s*$', line): - function_state.Count() # Count non-blank/non-comment lines. - - -_RE_PATTERN_TODO = re.compile(r'^//(\s*)TODO(\(.+?\))?:?(\s|$)?') - - -def CheckComment(line, filename, linenum, next_line_start, error): - """Checks for common mistakes in comments. - - Args: - line: The line in question. - filename: The name of the current file. - linenum: The number of the line to check. - next_line_start: The first non-whitespace column of the next line. - error: The function to call with any errors found. - """ - commentpos = line.find('//') - if commentpos != -1: - # Check if the // may be in quotes. If so, ignore it - if re.sub(r'\\.', '', line[0:commentpos]).count('"') % 2 == 0: - # Allow one space for new scopes, two spaces otherwise: - if (not (Match(r'^.*{ *//', line) and next_line_start == commentpos) and - ((commentpos >= 1 and - line[commentpos-1] not in string.whitespace) or - (commentpos >= 2 and - line[commentpos-2] not in string.whitespace))): - error(filename, linenum, 'whitespace/comments', 2, - 'At least two spaces is best between code and comments') - - # Checks for common mistakes in TODO comments. - comment = line[commentpos:] - match = _RE_PATTERN_TODO.match(comment) - if match: - # One whitespace is correct; zero whitespace is handled elsewhere. - leading_whitespace = match.group(1) - if len(leading_whitespace) > 1: - error(filename, linenum, 'whitespace/todo', 2, - 'Too many spaces before TODO') - - username = match.group(2) - if not username: - error(filename, linenum, 'readability/todo', 2, - 'Missing username in TODO; it should look like ' - '"// TODO(my_username): Stuff."') - - middle_whitespace = match.group(3) - # Comparisons made explicit for correctness -- pylint: disable=g-explicit-bool-comparison - if middle_whitespace != ' ' and middle_whitespace != '': - error(filename, linenum, 'whitespace/todo', 2, - 'TODO(my_username) should be followed by a space') - - # If the comment contains an alphanumeric character, there - # should be a space somewhere between it and the // unless - # it's a /// or //! Doxygen comment. - if (Match(r'//[^ ]*\w', comment) and - not Match(r'(///|//\!)(\s+|$)', comment)): - error(filename, linenum, 'whitespace/comments', 4, - 'Should have a space between // and comment') - - -def CheckSpacing(filename, clean_lines, linenum, nesting_state, error): - """Checks for the correctness of various spacing issues in the code. - - Things we check for: spaces around operators, spaces after - if/for/while/switch, no spaces around parens in function calls, two - spaces between code and comment, don't start a block with a blank - line, don't end a function with a blank line, don't add a blank line - after public/protected/private, don't have too many blank lines in a row. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: The function to call with any errors found. - """ - - # Don't use "elided" lines here, otherwise we can't check commented lines. - # Don't want to use "raw" either, because we don't want to check inside C++11 - # raw strings, - raw = clean_lines.lines_without_raw_strings - line = raw[linenum] - - # Before nixing comments, check if the line is blank for no good - # reason. This includes the first line after a block is opened, and - # blank lines at the end of a function (ie, right before a line like '}' - # - # Skip all the blank line checks if we are immediately inside a - # namespace body. In other words, don't issue blank line warnings - # for this block: - # namespace { - # - # } - # - # A warning about missing end of namespace comments will be issued instead. - # - # Also skip blank line checks for 'extern "C"' blocks, which are formatted - # like namespaces. - if (IsBlankLine(line) and - not nesting_state.InNamespaceBody() and - not nesting_state.InExternC()): - elided = clean_lines.elided - prev_line = elided[linenum - 1] - prevbrace = prev_line.rfind('{') - # TODO(unknown): Don't complain if line before blank line, and line after, - # both start with alnums and are indented the same amount. - # This ignores whitespace at the start of a namespace block - # because those are not usually indented. - if prevbrace != -1 and prev_line[prevbrace:].find('}') == -1: - # OK, we have a blank line at the start of a code block. Before we - # complain, we check if it is an exception to the rule: The previous - # non-empty line has the parameters of a function header that are indented - # 4 spaces (because they did not fit in a 80 column line when placed on - # the same line as the function name). We also check for the case where - # the previous line is indented 6 spaces, which may happen when the - # initializers of a constructor do not fit into a 80 column line. - exception = False - if Match(r' {6}\w', prev_line): # Initializer list? - # We are looking for the opening column of initializer list, which - # should be indented 4 spaces to cause 6 space indentation afterwards. - search_position = linenum-2 - while (search_position >= 0 - and Match(r' {6}\w', elided[search_position])): - search_position -= 1 - exception = (search_position >= 0 - and elided[search_position][:5] == ' :') - else: - # Search for the function arguments or an initializer list. We use a - # simple heuristic here: If the line is indented 4 spaces; and we have a - # closing paren, without the opening paren, followed by an opening brace - # or colon (for initializer lists) we assume that it is the last line of - # a function header. If we have a colon indented 4 spaces, it is an - # initializer list. - exception = (Match(r' {4}\w[^\(]*\)\s*(const\s*)?(\{\s*$|:)', - prev_line) - or Match(r' {4}:', prev_line)) - - if not exception: - error(filename, linenum, 'whitespace/blank_line', 2, - 'Redundant blank line at the start of a code block ' - 'should be deleted.') - # Ignore blank lines at the end of a block in a long if-else - # chain, like this: - # if (condition1) { - # // Something followed by a blank line - # - # } else if (condition2) { - # // Something else - # } - if linenum + 1 < clean_lines.NumLines(): - next_line = raw[linenum + 1] - if (next_line - and Match(r'\s*}', next_line) - and next_line.find('} else ') == -1): - error(filename, linenum, 'whitespace/blank_line', 3, - 'Redundant blank line at the end of a code block ' - 'should be deleted.') - - matched = Match(r'\s*(public|protected|private):', prev_line) - # if matched: - # error(filename, linenum, 'whitespace/blank_line', 3, - # 'Do not leave a blank line after "%s:"' % matched.group(1)) - - # Next, check comments - next_line_start = 0 - if linenum + 1 < clean_lines.NumLines(): - next_line = raw[linenum + 1] - next_line_start = len(next_line) - len(next_line.lstrip()) - CheckComment(line, filename, linenum, next_line_start, error) - - # get rid of comments and strings - line = clean_lines.elided[linenum] - - # You shouldn't have spaces before your brackets, except maybe after - # 'delete []' or 'return []() {};' - if Search(r'\w\s+\[', line) and not Search(r'(?:delete|return)\s+\[', line): - error(filename, linenum, 'whitespace/braces', 5, - 'Extra space before [') - - # In range-based for, we wanted spaces before and after the colon, but - # not around "::" tokens that might appear. - if (Search(r'for *\(.*[^:]:[^: ]', line) or - Search(r'for *\(.*[^: ]:[^:]', line)): - error(filename, linenum, 'whitespace/forcolon', 2, - 'Missing space around colon in range-based for loop') - - -def CheckOperatorSpacing(filename, clean_lines, linenum, error): - """Checks for horizontal spacing around operators. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Don't try to do spacing checks for operator methods. Do this by - # replacing the troublesome characters with something else, - # preserving column position for all other characters. - # - # The replacement is done repeatedly to avoid false positives from - # operators that call operators. - while True: - match = Match(r'^(.*\boperator\b)(\S+)(\s*\(.*)$', line) - if match: - line = match.group(1) + ('_' * len(match.group(2))) + match.group(3) - else: - break - - # We allow no-spaces around = within an if: "if ( (a=Foo()) == 0 )". - # Otherwise not. Note we only check for non-spaces on *both* sides; - # sometimes people put non-spaces on one side when aligning ='s among - # many lines (not that this is behavior that I approve of...) - if ((Search(r'[\w.]=', line) or - Search(r'=[\w.]', line)) - and not Search(r'\b(if|while|for) ', line) - # Operators taken from [lex.operators] in C++11 standard. - and not Search(r'(>=|<=|==|!=|&=|\^=|\|=|\+=|\*=|\/=|\%=)', line) - and not Search(r'operator=', line)): - error(filename, linenum, 'whitespace/operators', 4, - 'Missing spaces around =') - - # It's ok not to have spaces around binary operators like + - * /, but if - # there's too little whitespace, we get concerned. It's hard to tell, - # though, so we punt on this one for now. TODO. - - # You should always have whitespace around binary operators. - # - # Check <= and >= first to avoid false positives with < and >, then - # check non-include lines for spacing around < and >. - # - # If the operator is followed by a comma, assume it's be used in a - # macro context and don't do any checks. This avoids false - # positives. - # - # Note that && is not included here. This is because there are too - # many false positives due to RValue references. - match = Search(r'[^<>=!\s](==|!=|<=|>=|\|\|)[^<>=!\s,;\)]', line) - if match: - error(filename, linenum, 'whitespace/operators', 3, - 'Missing spaces around %s' % match.group(1)) - elif not Match(r'#.*include', line): - # Look for < that is not surrounded by spaces. This is only - # triggered if both sides are missing spaces, even though - # technically should should flag if at least one side is missing a - # space. This is done to avoid some false positives with shifts. - match = Match(r'^(.*[^\s<])<[^\s=<,]', line) - if match: - (_, _, end_pos) = CloseExpression( - clean_lines, linenum, len(match.group(1))) - if end_pos <= -1: - error(filename, linenum, 'whitespace/operators', 3, - 'Missing spaces around <') - - # Look for > that is not surrounded by spaces. Similar to the - # above, we only trigger if both sides are missing spaces to avoid - # false positives with shifts. - match = Match(r'^(.*[^-\s>])>[^\s=>,]', line) - if match: - (_, _, start_pos) = ReverseCloseExpression( - clean_lines, linenum, len(match.group(1))) - if start_pos <= -1: - error(filename, linenum, 'whitespace/operators', 3, - 'Missing spaces around >') - - # We allow no-spaces around << when used like this: 10<<20, but - # not otherwise (particularly, not when used as streams) - # - # We also allow operators following an opening parenthesis, since - # those tend to be macros that deal with operators. - match = Search(r'(operator|[^\s(<])(?:L|UL|LL|ULL|l|ul|ll|ull)?<<([^\s,=<])', line) - if (match and not (match.group(1).isdigit() and match.group(2).isdigit()) and - not (match.group(1) == 'operator' and match.group(2) == ';')): - error(filename, linenum, 'whitespace/operators', 3, - 'Missing spaces around <<') - - # We allow no-spaces around >> for almost anything. This is because - # C++11 allows ">>" to close nested templates, which accounts for - # most cases when ">>" is not followed by a space. - # - # We still warn on ">>" followed by alpha character, because that is - # likely due to ">>" being used for right shifts, e.g.: - # value >> alpha - # - # When ">>" is used to close templates, the alphanumeric letter that - # follows would be part of an identifier, and there should still be - # a space separating the template type and the identifier. - # type> alpha - match = Search(r'>>[a-zA-Z_]', line) - if match: - error(filename, linenum, 'whitespace/operators', 3, - 'Missing spaces around >>') - - # There shouldn't be space around unary operators - match = Search(r'(!\s|~\s|[\s]--[\s;]|[\s]\+\+[\s;])', line) - if match: - error(filename, linenum, 'whitespace/operators', 4, - 'Extra space for operator %s' % match.group(1)) - - -def CheckParenthesisSpacing(filename, clean_lines, linenum, error): - """Checks for horizontal spacing around parentheses. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # No spaces after an if, while, switch, or for - match = Search(r' (if\(|for\(|while\(|switch\()', line) - if match: - error(filename, linenum, 'whitespace/parens', 5, - 'Missing space before ( in %s' % match.group(1)) - - # For if/for/while/switch, the left and right parens should be - # consistent about how many spaces are inside the parens, and - # there should either be zero or one spaces inside the parens. - # We don't want: "if ( foo)" or "if ( foo )". - # Exception: "for ( ; foo; bar)" and "for (foo; bar; )" are allowed. - match = Search(r'\b(if|for|while|switch)\s*' - r'\(([ ]*)(.).*[^ ]+([ ]*)\)\s*{\s*$', - line) - if match: - if len(match.group(2)) != len(match.group(4)): - if not (match.group(3) == ';' and - len(match.group(2)) == 1 + len(match.group(4)) or - not match.group(2) and Search(r'\bfor\s*\(.*; \)', line)): - error(filename, linenum, 'whitespace/parens', 5, - 'Mismatching spaces inside () in %s' % match.group(1)) - if len(match.group(2)) not in [0, 1]: - error(filename, linenum, 'whitespace/parens', 5, - 'Should have zero or one spaces inside ( and ) in %s' % - match.group(1)) - - -def CheckCommaSpacing(filename, clean_lines, linenum, error): - """Checks for horizontal spacing near commas and semicolons. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - raw = clean_lines.lines_without_raw_strings - line = clean_lines.elided[linenum] - - # You should always have a space after a comma (either as fn arg or operator) - # - # This does not apply when the non-space character following the - # comma is another comma, since the only time when that happens is - # for empty macro arguments. - # - # We run this check in two passes: first pass on elided lines to - # verify that lines contain missing whitespaces, second pass on raw - # lines to confirm that those missing whitespaces are not due to - # elided comments. - if (Search(r',[^,\s]', ReplaceAll(r'\boperator\s*,\s*\(', 'F(', line)) and - Search(r',[^,\s]', raw[linenum])): - error(filename, linenum, 'whitespace/comma', 3, - 'Missing space after ,') - - # You should always have a space after a semicolon - # except for few corner cases - # TODO(unknown): clarify if 'if (1) { return 1;}' is requires one more - # space after ; - if Search(r';[^\s};\\)/]', line): - error(filename, linenum, 'whitespace/semicolon', 3, - 'Missing space after ;') - - -def _IsType(clean_lines, nesting_state, expr): - """Check if expression looks like a type name, returns true if so. - - Args: - clean_lines: A CleansedLines instance containing the file. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - expr: The expression to check. - Returns: - True, if token looks like a type. - """ - # Keep only the last token in the expression - last_word = Match(r'^.*(\b\S+)$', expr) - if last_word: - token = last_word.group(1) - else: - token = expr - - # Match native types and stdint types - if _TYPES.match(token): - return True - - # Try a bit harder to match templated types. Walk up the nesting - # stack until we find something that resembles a typename - # declaration for what we are looking for. - typename_pattern = (r'\b(?:typename|class|struct)\s+' + re.escape(token) + - r'\b') - block_index = len(nesting_state.stack) - 1 - while block_index >= 0: - if isinstance(nesting_state.stack[block_index], _NamespaceInfo): - return False - - # Found where the opening brace is. We want to scan from this - # line up to the beginning of the function, minus a few lines. - # template - # class C - # : public ... { // start scanning here - last_line = nesting_state.stack[block_index].starting_linenum - - next_block_start = 0 - if block_index > 0: - next_block_start = nesting_state.stack[block_index - 1].starting_linenum - first_line = last_line - while first_line >= next_block_start: - if clean_lines.elided[first_line].find('template') >= 0: - break - first_line -= 1 - if first_line < next_block_start: - # Didn't find any "template" keyword before reaching the next block, - # there are probably no template things to check for this block - block_index -= 1 - continue - - # Look for typename in the specified range - for i in xrange(first_line, last_line + 1, 1): - if Search(typename_pattern, clean_lines.elided[i]): - return True - block_index -= 1 - - return False - - -def CheckBracesSpacing(filename, clean_lines, linenum, nesting_state, error): - """Checks for horizontal spacing near commas. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Except after an opening paren, or after another opening brace (in case of - # an initializer list, for instance), you should have spaces before your - # braces when they are delimiting blocks, classes, namespaces etc. - # And since you should never have braces at the beginning of a line, - # this is an easy test. Except that braces used for initialization don't - # follow the same rule; we often don't want spaces before those. - match = Match(r'^(.*[^ ({>]){', line) - - if match: - # Try a bit harder to check for brace initialization. This - # happens in one of the following forms: - # Constructor() : initializer_list_{} { ... } - # Constructor{}.MemberFunction() - # Type variable{}; - # FunctionCall(type{}, ...); - # LastArgument(..., type{}); - # LOG(INFO) << type{} << " ..."; - # map_of_type[{...}] = ...; - # ternary = expr ? new type{} : nullptr; - # OuterTemplate{}> - # - # We check for the character following the closing brace, and - # silence the warning if it's one of those listed above, i.e. - # "{.;,)<>]:". - # - # To account for nested initializer list, we allow any number of - # closing braces up to "{;,)<". We can't simply silence the - # warning on first sight of closing brace, because that would - # cause false negatives for things that are not initializer lists. - # Silence this: But not this: - # Outer{ if (...) { - # Inner{...} if (...){ // Missing space before { - # }; } - # - # There is a false negative with this approach if people inserted - # spurious semicolons, e.g. "if (cond){};", but we will catch the - # spurious semicolon with a separate check. - leading_text = match.group(1) - (endline, endlinenum, endpos) = CloseExpression( - clean_lines, linenum, len(match.group(1))) - trailing_text = '' - if endpos > -1: - trailing_text = endline[endpos:] - for offset in xrange(endlinenum + 1, - min(endlinenum + 3, clean_lines.NumLines() - 1)): - trailing_text += clean_lines.elided[offset] - # We also suppress warnings for `uint64_t{expression}` etc., as the style - # guide recommends brace initialization for integral types to avoid - # overflow/truncation. - if (not Match(r'^[\s}]*[{.;,)<>\]:]', trailing_text) - and not _IsType(clean_lines, nesting_state, leading_text)): - error(filename, linenum, 'whitespace/braces', 5, - 'Missing space before {') - - # Make sure '} else {' has spaces. - if Search(r'}else', line): - error(filename, linenum, 'whitespace/braces', 5, - 'Missing space before else') - - # You shouldn't have a space before a semicolon at the end of the line. - # There's a special case for "for" since the style guide allows space before - # the semicolon there. - if Search(r':\s*;\s*$', line): - error(filename, linenum, 'whitespace/semicolon', 5, - 'Semicolon defining empty statement. Use {} instead.') - elif Search(r'^\s*;\s*$', line): - error(filename, linenum, 'whitespace/semicolon', 5, - 'Line contains only semicolon. If this should be an empty statement, ' - 'use {} instead.') - elif (Search(r'\s+;\s*$', line) and - not Search(r'\bfor\b', line)): - error(filename, linenum, 'whitespace/semicolon', 5, - 'Extra space before last semicolon. If this should be an empty ' - 'statement, use {} instead.') - - -def IsDecltype(clean_lines, linenum, column): - """Check if the token ending on (linenum, column) is decltype(). - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: the number of the line to check. - column: end column of the token to check. - Returns: - True if this token is decltype() expression, False otherwise. - """ - (text, _, start_col) = ReverseCloseExpression(clean_lines, linenum, column) - if start_col < 0: - return False - if Search(r'\bdecltype\s*$', text[0:start_col]): - return True - return False - - -def CheckSectionSpacing(filename, clean_lines, class_info, linenum, error): - """Checks for additional blank line issues related to sections. - - Currently the only thing checked here is blank line before protected/private. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - class_info: A _ClassInfo objects. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - # Skip checks if the class is small, where small means 25 lines or less. - # 25 lines seems like a good cutoff since that's the usual height of - # terminals, and any class that can't fit in one screen can't really - # be considered "small". - # - # Also skip checks if we are on the first line. This accounts for - # classes that look like - # class Foo { public: ... }; - # - # If we didn't find the end of the class, last_line would be zero, - # and the check will be skipped by the first condition. - if (class_info.last_line - class_info.starting_linenum <= 24 or - linenum <= class_info.starting_linenum): - return - - matched = Match(r'\s*(public|protected|private):', clean_lines.lines[linenum]) - if matched: - # Issue warning if the line before public/protected/private was - # not a blank line, but don't do this if the previous line contains - # "class" or "struct". This can happen two ways: - # - We are at the beginning of the class. - # - We are forward-declaring an inner class that is semantically - # private, but needed to be public for implementation reasons. - # Also ignores cases where the previous line ends with a backslash as can be - # common when defining classes in C macros. - prev_line = clean_lines.lines[linenum - 1] - if (not IsBlankLine(prev_line) and - not Search(r'\b(class|struct)\b', prev_line) and - not Search(r'\\$', prev_line)): - # Try a bit harder to find the beginning of the class. This is to - # account for multi-line base-specifier lists, e.g.: - # class Derived - # : public Base { - end_class_head = class_info.starting_linenum - for i in range(class_info.starting_linenum, linenum): - if Search(r'\{\s*$', clean_lines.lines[i]): - end_class_head = i - break - if end_class_head < linenum - 1: - error(filename, linenum, 'whitespace/blank_line', 3, - '"%s:" should be preceded by a blank line' % matched.group(1)) - - -def GetPreviousNonBlankLine(clean_lines, linenum): - """Return the most recent non-blank line and its line number. - - Args: - clean_lines: A CleansedLines instance containing the file contents. - linenum: The number of the line to check. - - Returns: - A tuple with two elements. The first element is the contents of the last - non-blank line before the current line, or the empty string if this is the - first non-blank line. The second is the line number of that line, or -1 - if this is the first non-blank line. - """ - - prevlinenum = linenum - 1 - while prevlinenum >= 0: - prevline = clean_lines.elided[prevlinenum] - if not IsBlankLine(prevline): # if not a blank line... - return (prevline, prevlinenum) - prevlinenum -= 1 - return ('', -1) - - -def CheckBraces(filename, clean_lines, linenum, error): - """Looks for misplaced braces (e.g. at the end of line). - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - - line = clean_lines.elided[linenum] # get rid of comments and strings - - if Match(r'\s*{\s*$', line): - # We allow an open brace to start a line in the case where someone is using - # braces in a block to explicitly create a new scope, which is commonly used - # to control the lifetime of stack-allocated variables. Braces are also - # used for brace initializers inside function calls. We don't detect this - # perfectly: we just don't complain if the last non-whitespace character on - # the previous non-blank line is ',', ';', ':', '(', '{', or '}', or if the - # previous line starts a preprocessor block. We also allow a brace on the - # following line if it is part of an array initialization and would not fit - # within the 80 character limit of the preceding line. - prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0] - # if (not Search(r'[,;:}{(]\s*$', prevline) and - # not Match(r'\s*#', prevline) and - # not (GetLineWidth(prevline) > _line_length - 2 and '[]' in prevline)): - # error(filename, linenum, 'whitespace/braces', 4, - # '{ should almost always be at the end of the previous line') - - # An else clause should be on the same line as the preceding closing brace. - if Match(r'\s*}\s*else\b\s*(?:if\b|\{|$)', line): - error(filename, linenum, 'whitespace/newline', 4, - 'An else should appear on a new line after }') - - # If braces come on one side of an else, they should be on both. - # However, we have to worry about "else if" that spans multiple lines! - if Search(r'else if\s*\(', line): # could be multi-line if - brace_on_left = bool(Search(r'}\s*else if\s*\(', line)) - # find the ( after the if - pos = line.find('else if') - pos = line.find('(', pos) - if pos > 0: - (endline, _, endpos) = CloseExpression(clean_lines, linenum, pos) - brace_on_right = endline[endpos:].find('{') != -1 - if brace_on_left != brace_on_right: # must be brace after if - error(filename, linenum, 'readability/braces', 5, - 'If an else has a brace on one side, it should have it on both') - elif Search(r'}\s*else[^{]*$', line) or Match(r'[^}]*else\s*{', line): - error(filename, linenum, 'readability/braces', 5, - 'If an else has a brace on one side, it should have it on both') - - # Likewise, an else should never have the else clause on the same line - if Search(r'\belse [^\s{]', line) and not Search(r'\belse if\b', line): - error(filename, linenum, 'whitespace/newline', 4, - 'Else clause should never be on same line as else (use 2 lines)') - - # In the same way, a do/while should never be on one line - if Match(r'\s*do [^\s{]', line): - error(filename, linenum, 'whitespace/newline', 4, - 'do/while clauses should not be on a single line') - - # Check single-line if/else bodies. The style guide says 'curly braces are not - # required for single-line statements'. We additionally allow multi-line, - # single statements, but we reject anything with more than one semicolon in - # it. This means that the first semicolon after the if should be at the end of - # its line, and the line after that should have an indent level equal to or - # lower than the if. We also check for ambiguous if/else nesting without - # braces. - if_else_match = Search(r'\b(if\s*\(|else\b)', line) - if if_else_match and not Match(r'\s*#', line): - if_indent = GetIndentLevel(line) - endline, endlinenum, endpos = line, linenum, if_else_match.end() - if_match = Search(r'\bif\s*\(', line) - if if_match: - # This could be a multiline if condition, so find the end first. - pos = if_match.end() - 1 - (endline, endlinenum, endpos) = CloseExpression(clean_lines, linenum, pos) - # Check for an opening brace, either directly after the if or on the next - # line. If found, this isn't a single-statement conditional. - if (not Match(r'\s*{', endline[endpos:]) - and not (Match(r'\s*$', endline[endpos:]) - and endlinenum < (len(clean_lines.elided) - 1) - and Match(r'\s*{', clean_lines.elided[endlinenum + 1]))): - while (endlinenum < len(clean_lines.elided) - and ';' not in clean_lines.elided[endlinenum][endpos:]): - endlinenum += 1 - endpos = 0 - if endlinenum < len(clean_lines.elided): - endline = clean_lines.elided[endlinenum] - # We allow a mix of whitespace and closing braces (e.g. for one-liner - # methods) and a single \ after the semicolon (for macros) - endpos = endline.find(';') - if not Match(r';[\s}]*(\\?)$', endline[endpos:]): - # Semicolon isn't the last character, there's something trailing. - # Output a warning if the semicolon is not contained inside - # a lambda expression. - if not Match(r'^[^{};]*\[[^\[\]]*\][^{}]*\{[^{}]*\}\s*\)*[;,]\s*$', - endline): - error(filename, linenum, 'readability/braces', 4, - 'If/else bodies with multiple statements require braces') - elif endlinenum < len(clean_lines.elided) - 1: - # Make sure the next line is dedented - next_line = clean_lines.elided[endlinenum + 1] - next_indent = GetIndentLevel(next_line) - # With ambiguous nested if statements, this will error out on the - # if that *doesn't* match the else, regardless of whether it's the - # inner one or outer one. - if (if_match and Match(r'\s*else\b', next_line) - and next_indent != if_indent): - error(filename, linenum, 'readability/braces', 4, - 'Else clause should be indented at the same level as if. ' - 'Ambiguous nested if/else chains require braces.') - elif next_indent > if_indent: - error(filename, linenum, 'readability/braces', 4, - 'If/else bodies with multiple statements require braces') - - -def CheckTrailingSemicolon(filename, clean_lines, linenum, error): - """Looks for redundant trailing semicolon. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - - line = clean_lines.elided[linenum] - - # Block bodies should not be followed by a semicolon. Due to C++11 - # brace initialization, there are more places where semicolons are - # required than not, so we use a whitelist approach to check these - # rather than a blacklist. These are the places where "};" should - # be replaced by just "}": - # 1. Some flavor of block following closing parenthesis: - # for (;;) {}; - # while (...) {}; - # switch (...) {}; - # Function(...) {}; - # if (...) {}; - # if (...) else if (...) {}; - # - # 2. else block: - # if (...) else {}; - # - # 3. const member function: - # Function(...) const {}; - # - # 4. Block following some statement: - # x = 42; - # {}; - # - # 5. Block at the beginning of a function: - # Function(...) { - # {}; - # } - # - # Note that naively checking for the preceding "{" will also match - # braces inside multi-dimensional arrays, but this is fine since - # that expression will not contain semicolons. - # - # 6. Block following another block: - # while (true) {} - # {}; - # - # 7. End of namespaces: - # namespace {}; - # - # These semicolons seems far more common than other kinds of - # redundant semicolons, possibly due to people converting classes - # to namespaces. For now we do not warn for this case. - # - # Try matching case 1 first. - match = Match(r'^(.*\)\s*)\{', line) - if match: - # Matched closing parenthesis (case 1). Check the token before the - # matching opening parenthesis, and don't warn if it looks like a - # macro. This avoids these false positives: - # - macro that defines a base class - # - multi-line macro that defines a base class - # - macro that defines the whole class-head - # - # But we still issue warnings for macros that we know are safe to - # warn, specifically: - # - TEST, TEST_F, TEST_P, MATCHER, MATCHER_P - # - TYPED_TEST - # - INTERFACE_DEF - # - EXCLUSIVE_LOCKS_REQUIRED, SHARED_LOCKS_REQUIRED, LOCKS_EXCLUDED: - # - # We implement a whitelist of safe macros instead of a blacklist of - # unsafe macros, even though the latter appears less frequently in - # google code and would have been easier to implement. This is because - # the downside for getting the whitelist wrong means some extra - # semicolons, while the downside for getting the blacklist wrong - # would result in compile errors. - # - # In addition to macros, we also don't want to warn on - # - Compound literals - # - Lambdas - # - alignas specifier with anonymous structs - # - decltype - closing_brace_pos = match.group(1).rfind(')') - opening_parenthesis = ReverseCloseExpression( - clean_lines, linenum, closing_brace_pos) - if opening_parenthesis[2] > -1: - line_prefix = opening_parenthesis[0][0:opening_parenthesis[2]] - macro = Search(r'\b([A-Z_][A-Z0-9_]*)\s*$', line_prefix) - func = Match(r'^(.*\])\s*$', line_prefix) - if ((macro and - macro.group(1) not in ( - 'TEST', 'TEST_F', 'MATCHER', 'MATCHER_P', 'TYPED_TEST', - 'EXCLUSIVE_LOCKS_REQUIRED', 'SHARED_LOCKS_REQUIRED', - 'LOCKS_EXCLUDED', 'INTERFACE_DEF')) or - (func and not Search(r'\boperator\s*\[\s*\]', func.group(1))) or - Search(r'\b(?:struct|union)\s+alignas\s*$', line_prefix) or - Search(r'\bdecltype$', line_prefix) or - Search(r'\s+=\s*$', line_prefix)): - match = None - if (match and - opening_parenthesis[1] > 1 and - Search(r'\]\s*$', clean_lines.elided[opening_parenthesis[1] - 1])): - # Multi-line lambda-expression - match = None - - else: - # Try matching cases 2-3. - match = Match(r'^(.*(?:else|\)\s*const)\s*)\{', line) - if not match: - # Try matching cases 4-6. These are always matched on separate lines. - # - # Note that we can't simply concatenate the previous line to the - # current line and do a single match, otherwise we may output - # duplicate warnings for the blank line case: - # if (cond) { - # // blank line - # } - prevline = GetPreviousNonBlankLine(clean_lines, linenum)[0] - if prevline and Search(r'[;{}]\s*$', prevline): - match = Match(r'^(\s*)\{', line) - - # Check matching closing brace - if match: - (endline, endlinenum, endpos) = CloseExpression( - clean_lines, linenum, len(match.group(1))) - if endpos > -1 and Match(r'^\s*;', endline[endpos:]): - # Current {} pair is eligible for semicolon check, and we have found - # the redundant semicolon, output warning here. - # - # Note: because we are scanning forward for opening braces, and - # outputting warnings for the matching closing brace, if there are - # nested blocks with trailing semicolons, we will get the error - # messages in reversed order. - - # We need to check the line forward for NOLINT - raw_lines = clean_lines.raw_lines - ParseNolintSuppressions(filename, raw_lines[endlinenum-1], endlinenum-1, - error) - ParseNolintSuppressions(filename, raw_lines[endlinenum], endlinenum, - error) - - error(filename, endlinenum, 'readability/braces', 4, - "You don't need a ; after a }") - - -def CheckEmptyBlockBody(filename, clean_lines, linenum, error): - """Look for empty loop/conditional body with only a single semicolon. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - - # Search for loop keywords at the beginning of the line. Because only - # whitespaces are allowed before the keywords, this will also ignore most - # do-while-loops, since those lines should start with closing brace. - # - # We also check "if" blocks here, since an empty conditional block - # is likely an error. - line = clean_lines.elided[linenum] - matched = Match(r'\s*(for|while|if)\s*\(', line) - if matched: - # Find the end of the conditional expression. - (end_line, end_linenum, end_pos) = CloseExpression( - clean_lines, linenum, line.find('(')) - - # Output warning if what follows the condition expression is a semicolon. - # No warning for all other cases, including whitespace or newline, since we - # have a separate check for semicolons preceded by whitespace. - if end_pos >= 0 and Match(r';', end_line[end_pos:]): - if matched.group(1) == 'if': - error(filename, end_linenum, 'whitespace/empty_conditional_body', 5, - 'Empty conditional bodies should use {}') - else: - error(filename, end_linenum, 'whitespace/empty_loop_body', 5, - 'Empty loop bodies should use {} or continue') - - # Check for if statements that have completely empty bodies (no comments) - # and no else clauses. - if end_pos >= 0 and matched.group(1) == 'if': - # Find the position of the opening { for the if statement. - # Return without logging an error if it has no brackets. - opening_linenum = end_linenum - opening_line_fragment = end_line[end_pos:] - # Loop until EOF or find anything that's not whitespace or opening {. - while not Search(r'^\s*\{', opening_line_fragment): - if Search(r'^(?!\s*$)', opening_line_fragment): - # Conditional has no brackets. - return - opening_linenum += 1 - if opening_linenum == len(clean_lines.elided): - # Couldn't find conditional's opening { or any code before EOF. - return - opening_line_fragment = clean_lines.elided[opening_linenum] - # Set opening_line (opening_line_fragment may not be entire opening line). - opening_line = clean_lines.elided[opening_linenum] - - # Find the position of the closing }. - opening_pos = opening_line_fragment.find('{') - if opening_linenum == end_linenum: - # We need to make opening_pos relative to the start of the entire line. - opening_pos += end_pos - (closing_line, closing_linenum, closing_pos) = CloseExpression( - clean_lines, opening_linenum, opening_pos) - if closing_pos < 0: - return - - # Now construct the body of the conditional. This consists of the portion - # of the opening line after the {, all lines until the closing line, - # and the portion of the closing line before the }. - if (clean_lines.raw_lines[opening_linenum] != - CleanseComments(clean_lines.raw_lines[opening_linenum])): - # Opening line ends with a comment, so conditional isn't empty. - return - if closing_linenum > opening_linenum: - # Opening line after the {. Ignore comments here since we checked above. - body = list(opening_line[opening_pos+1:]) - # All lines until closing line, excluding closing line, with comments. - body.extend(clean_lines.raw_lines[opening_linenum+1:closing_linenum]) - # Closing line before the }. Won't (and can't) have comments. - body.append(clean_lines.elided[closing_linenum][:closing_pos-1]) - body = '\n'.join(body) - else: - # If statement has brackets and fits on a single line. - body = opening_line[opening_pos+1:closing_pos-1] - - # Check if the body is empty - if not _EMPTY_CONDITIONAL_BODY_PATTERN.search(body): - return - # The body is empty. Now make sure there's not an else clause. - current_linenum = closing_linenum - current_line_fragment = closing_line[closing_pos:] - # Loop until EOF or find anything that's not whitespace or else clause. - while Search(r'^\s*$|^(?=\s*else)', current_line_fragment): - if Search(r'^(?=\s*else)', current_line_fragment): - # Found an else clause, so don't log an error. - return - current_linenum += 1 - if current_linenum == len(clean_lines.elided): - break - current_line_fragment = clean_lines.elided[current_linenum] - - # The body is empty and there's no else clause until EOF or other code. - error(filename, end_linenum, 'whitespace/empty_if_body', 4, - ('If statement had no body and no else clause')) - - -def FindCheckMacro(line): - """Find a replaceable CHECK-like macro. - - Args: - line: line to search on. - Returns: - (macro name, start position), or (None, -1) if no replaceable - macro is found. - """ - for macro in _CHECK_MACROS: - i = line.find(macro) - if i >= 0: - # Find opening parenthesis. Do a regular expression match here - # to make sure that we are matching the expected CHECK macro, as - # opposed to some other macro that happens to contain the CHECK - # substring. - matched = Match(r'^(.*\b' + macro + r'\s*)\(', line) - if not matched: - continue - return (macro, len(matched.group(1))) - return (None, -1) - - -def CheckCheck(filename, clean_lines, linenum, error): - """Checks the use of CHECK and EXPECT macros. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - - # Decide the set of replacement macros that should be suggested - lines = clean_lines.elided - (check_macro, start_pos) = FindCheckMacro(lines[linenum]) - if not check_macro: - return - - # Find end of the boolean expression by matching parentheses - (last_line, end_line, end_pos) = CloseExpression( - clean_lines, linenum, start_pos) - if end_pos < 0: - return - - # If the check macro is followed by something other than a - # semicolon, assume users will log their own custom error messages - # and don't suggest any replacements. - if not Match(r'\s*;', last_line[end_pos:]): - return - - if linenum == end_line: - expression = lines[linenum][start_pos + 1:end_pos - 1] - else: - expression = lines[linenum][start_pos + 1:] - for i in xrange(linenum + 1, end_line): - expression += lines[i] - expression += last_line[0:end_pos - 1] - - # Parse expression so that we can take parentheses into account. - # This avoids false positives for inputs like "CHECK((a < 4) == b)", - # which is not replaceable by CHECK_LE. - lhs = '' - rhs = '' - operator = None - while expression: - matched = Match(r'^\s*(<<|<<=|>>|>>=|->\*|->|&&|\|\||' - r'==|!=|>=|>|<=|<|\()(.*)$', expression) - if matched: - token = matched.group(1) - if token == '(': - # Parenthesized operand - expression = matched.group(2) - (end, _) = FindEndOfExpressionInLine(expression, 0, ['(']) - if end < 0: - return # Unmatched parenthesis - lhs += '(' + expression[0:end] - expression = expression[end:] - elif token in ('&&', '||'): - # Logical and/or operators. This means the expression - # contains more than one term, for example: - # CHECK(42 < a && a < b); - # - # These are not replaceable with CHECK_LE, so bail out early. - return - elif token in ('<<', '<<=', '>>', '>>=', '->*', '->'): - # Non-relational operator - lhs += token - expression = matched.group(2) - else: - # Relational operator - operator = token - rhs = matched.group(2) - break - else: - # Unparenthesized operand. Instead of appending to lhs one character - # at a time, we do another regular expression match to consume several - # characters at once if possible. Trivial benchmark shows that this - # is more efficient when the operands are longer than a single - # character, which is generally the case. - matched = Match(r'^([^-=!<>()&|]+)(.*)$', expression) - if not matched: - matched = Match(r'^(\s*\S)(.*)$', expression) - if not matched: - break - lhs += matched.group(1) - expression = matched.group(2) - - # Only apply checks if we got all parts of the boolean expression - if not (lhs and operator and rhs): - return - - # Check that rhs do not contain logical operators. We already know - # that lhs is fine since the loop above parses out && and ||. - if rhs.find('&&') > -1 or rhs.find('||') > -1: - return - - # At least one of the operands must be a constant literal. This is - # to avoid suggesting replacements for unprintable things like - # CHECK(variable != iterator) - # - # The following pattern matches decimal, hex integers, strings, and - # characters (in that order). - lhs = lhs.strip() - rhs = rhs.strip() - match_constant = r'^([-+]?(\d+|0[xX][0-9a-fA-F]+)[lLuU]{0,3}|".*"|\'.*\')$' - if Match(match_constant, lhs) or Match(match_constant, rhs): - # Note: since we know both lhs and rhs, we can provide a more - # descriptive error message like: - # Consider using CHECK_EQ(x, 42) instead of CHECK(x == 42) - # Instead of: - # Consider using CHECK_EQ instead of CHECK(a == b) - # - # We are still keeping the less descriptive message because if lhs - # or rhs gets long, the error message might become unreadable. - error(filename, linenum, 'readability/check', 2, - 'Consider using %s instead of %s(a %s b)' % ( - _CHECK_REPLACEMENT[check_macro][operator], - check_macro, operator)) - - -def CheckAltTokens(filename, clean_lines, linenum, error): - """Check alternative keywords being used in boolean expressions. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Avoid preprocessor lines - if Match(r'^\s*#', line): - return - - # Last ditch effort to avoid multi-line comments. This will not help - # if the comment started before the current line or ended after the - # current line, but it catches most of the false positives. At least, - # it provides a way to workaround this warning for people who use - # multi-line comments in preprocessor macros. - # - # TODO(unknown): remove this once cpplint has better support for - # multi-line comments. - if line.find('/*') >= 0 or line.find('*/') >= 0: - return - - for match in _ALT_TOKEN_REPLACEMENT_PATTERN.finditer(line): - error(filename, linenum, 'readability/alt_tokens', 2, - 'Use operator %s instead of %s' % ( - _ALT_TOKEN_REPLACEMENT[match.group(1)], match.group(1))) - - -def GetLineWidth(line): - """Determines the width of the line in column positions. - - Args: - line: A string, which may be a Unicode string. - - Returns: - The width of the line in column positions, accounting for Unicode - combining characters and wide characters. - """ - if isinstance(line, unicode): - width = 0 - for uc in unicodedata.normalize('NFC', line): - if unicodedata.east_asian_width(uc) in ('W', 'F'): - width += 2 - elif not unicodedata.combining(uc): - width += 1 - return width - else: - return len(line) - - -def CheckStyle(filename, clean_lines, linenum, file_extension, nesting_state, - error): - """Checks rules from the 'C++ style rules' section of cppguide.html. - - Most of these rules are hard to test (naming, comment style), but we - do what we can. In particular we check for 2-space indents, line lengths, - tab usage, spaces inside code, etc. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - file_extension: The extension (without the dot) of the filename. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: The function to call with any errors found. - """ - - # Don't use "elided" lines here, otherwise we can't check commented lines. - # Don't want to use "raw" either, because we don't want to check inside C++11 - # raw strings, - raw_lines = clean_lines.lines_without_raw_strings - line = raw_lines[linenum] - prev = raw_lines[linenum - 1] if linenum > 0 else '' - - if line.find('\t') != -1: - error(filename, linenum, 'whitespace/tab', 1, - 'Tab found; better to use spaces') - - # One or three blank spaces at the beginning of the line is weird; it's - # hard to reconcile that with 2-space indents. - # NOTE: here are the conditions rob pike used for his tests. Mine aren't - # as sophisticated, but it may be worth becoming so: RLENGTH==initial_spaces - # if(RLENGTH > 20) complain = 0; - # if(match($0, " +(error|private|public|protected):")) complain = 0; - # if(match(prev, "&& *$")) complain = 0; - # if(match(prev, "\\|\\| *$")) complain = 0; - # if(match(prev, "[\",=><] *$")) complain = 0; - # if(match($0, " <<")) complain = 0; - # if(match(prev, " +for \\(")) complain = 0; - # if(prevodd && match(prevprev, " +for \\(")) complain = 0; - scope_or_label_pattern = r'\s*\w+\s*:\s*\\?$' - classinfo = nesting_state.InnermostClass() - initial_spaces = 0 - cleansed_line = clean_lines.elided[linenum] - while initial_spaces < len(line) and line[initial_spaces] == ' ': - initial_spaces += 1 - # There are certain situations we allow one space, notably for - # section labels, and also lines containing multi-line raw strings. - # We also don't check for lines that look like continuation lines - # (of lines ending in double quotes, commas, equals, or angle brackets) - # because the rules for how to indent those are non-trivial. - if (not Search(r'[",=><] *$', prev) and - (initial_spaces == 1 or initial_spaces == 3) and - not Match(scope_or_label_pattern, cleansed_line) and - not (clean_lines.raw_lines[linenum] != line and - Match(r'^\s*""', line))): - error(filename, linenum, 'whitespace/indent', 3, - 'Weird number of spaces at line-start. ' - 'Are you using a 2-space indent?') - - if line and line[-1].isspace(): - error(filename, linenum, 'whitespace/end_of_line', 4, - 'Line ends in whitespace. Consider deleting these extra spaces.') - - # Check if the line is a header guard. - is_header_guard = False - if IsHeaderExtension(file_extension): - cppvar = GetHeaderGuardCPPVariable(filename) - if (line.startswith('#ifndef %s' % cppvar) or - line.startswith('#define %s' % cppvar) or - line.startswith('#endif // %s' % cppvar)): - is_header_guard = True - # #include lines and header guards can be long, since there's no clean way to - # split them. - # - # URLs can be long too. It's possible to split these, but it makes them - # harder to cut&paste. - # - # The "$Id:...$" comment may also get very long without it being the - # developers fault. - if (not line.startswith('#include') and not is_header_guard and - not Match(r'^\s*//.*http(s?)://\S*$', line) and - not Match(r'^\s*//\s*[^\s]*$', line) and - not Match(r'^// \$Id:.*#[0-9]+ \$$', line)): - line_width = GetLineWidth(line) - if line_width > _line_length: - error(filename, linenum, 'whitespace/line_length', 2, - 'Lines should be <= %i characters long' % _line_length) - - if (cleansed_line.count(';') > 1 and - # for loops are allowed two ;'s (and may run over two lines). - cleansed_line.find('for') == -1 and - (GetPreviousNonBlankLine(clean_lines, linenum)[0].find('for') == -1 or - GetPreviousNonBlankLine(clean_lines, linenum)[0].find(';') != -1) and - # It's ok to have many commands in a switch case that fits in 1 line - not ((cleansed_line.find('case ') != -1 or - cleansed_line.find('default:') != -1) and - cleansed_line.find('break;') != -1)): - error(filename, linenum, 'whitespace/newline', 0, - 'More than one command on the same line') - - # Some more style checks - CheckBraces(filename, clean_lines, linenum, error) - CheckTrailingSemicolon(filename, clean_lines, linenum, error) - CheckEmptyBlockBody(filename, clean_lines, linenum, error) - CheckSpacing(filename, clean_lines, linenum, nesting_state, error) - CheckOperatorSpacing(filename, clean_lines, linenum, error) - CheckParenthesisSpacing(filename, clean_lines, linenum, error) - CheckCommaSpacing(filename, clean_lines, linenum, error) - CheckBracesSpacing(filename, clean_lines, linenum, nesting_state, error) - CheckSpacingForFunctionCall(filename, clean_lines, linenum, error) - CheckCheck(filename, clean_lines, linenum, error) - CheckAltTokens(filename, clean_lines, linenum, error) - classinfo = nesting_state.InnermostClass() - if classinfo: - CheckSectionSpacing(filename, clean_lines, classinfo, linenum, error) - - -_RE_PATTERN_INCLUDE = re.compile(r'^\s*#\s*include\s*([<"])([^>"]*)[>"].*$') -# Matches the first component of a filename delimited by -s and _s. That is: -# _RE_FIRST_COMPONENT.match('foo').group(0) == 'foo' -# _RE_FIRST_COMPONENT.match('foo.cc').group(0) == 'foo' -# _RE_FIRST_COMPONENT.match('foo-bar_baz.cc').group(0) == 'foo' -# _RE_FIRST_COMPONENT.match('foo_bar-baz.cc').group(0) == 'foo' -_RE_FIRST_COMPONENT = re.compile(r'^[^-_.]+') - - -def _DropCommonSuffixes(filename): - """Drops common suffixes like _test.cc or -inl.h from filename. - - For example: - >>> _DropCommonSuffixes('foo/foo-inl.h') - 'foo/foo' - >>> _DropCommonSuffixes('foo/bar/foo.cc') - 'foo/bar/foo' - >>> _DropCommonSuffixes('foo/foo_internal.h') - 'foo/foo' - >>> _DropCommonSuffixes('foo/foo_unusualinternal.h') - 'foo/foo_unusualinternal' - - Args: - filename: The input filename. - - Returns: - The filename with the common suffix removed. - """ - for suffix in ('test.cc', 'regtest.cc', 'unittest.cc', - 'inl.h', 'impl.h', 'internal.h'): - if (filename.endswith(suffix) and len(filename) > len(suffix) and - filename[-len(suffix) - 1] in ('-', '_')): - return filename[:-len(suffix) - 1] - return os.path.splitext(filename)[0] - - -def _ClassifyInclude(fileinfo, include, is_system): - """Figures out what kind of header 'include' is. - - Args: - fileinfo: The current file cpplint is running over. A FileInfo instance. - include: The path to a #included file. - is_system: True if the #include used <> rather than "". - - Returns: - One of the _XXX_HEADER constants. - - For example: - >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'stdio.h', True) - _C_SYS_HEADER - >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'string', True) - _CPP_SYS_HEADER - >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'foo/foo.h', False) - _LIKELY_MY_HEADER - >>> _ClassifyInclude(FileInfo('foo/foo_unknown_extension.cc'), - ... 'bar/foo_other_ext.h', False) - _POSSIBLE_MY_HEADER - >>> _ClassifyInclude(FileInfo('foo/foo.cc'), 'foo/bar.h', False) - _OTHER_HEADER - """ - # This is a list of all standard c++ header files, except - # those already checked for above. - is_cpp_h = include in _CPP_HEADERS - - if is_system: - if is_cpp_h or include.endswith(".hh"): - return _CPP_SYS_HEADER - else: - return _C_SYS_HEADER - - # If the target file and the include we're checking share a - # basename when we drop common extensions, and the include - # lives in . , then it's likely to be owned by the target file. - target_dir, target_base = ( - os.path.split(_DropCommonSuffixes(fileinfo.RepositoryName()))) - include_dir, include_base = os.path.split(_DropCommonSuffixes(include)) - if target_base == include_base and ( - include_dir == target_dir or - include_dir == os.path.normpath(target_dir + '/../public')): - return _LIKELY_MY_HEADER - - # If the target and include share some initial basename - # component, it's possible the target is implementing the - # include, so it's allowed to be first, but we'll never - # complain if it's not there. - target_first_component = _RE_FIRST_COMPONENT.match(target_base) - include_first_component = _RE_FIRST_COMPONENT.match(include_base) - if (target_first_component and include_first_component and - target_first_component.group(0) == - include_first_component.group(0)): - return _POSSIBLE_MY_HEADER - - return _OTHER_HEADER - - - -def CheckIncludeLine(filename, clean_lines, linenum, include_state, error): - """Check rules that are applicable to #include lines. - - Strings on #include lines are NOT removed from elided line, to make - certain tasks easier. However, to prevent false positives, checks - applicable to #include lines in CheckLanguage must be put here. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - include_state: An _IncludeState instance in which the headers are inserted. - error: The function to call with any errors found. - """ - fileinfo = FileInfo(filename) - line = clean_lines.lines[linenum] - - # "include" should use the new style "foo/bar.h" instead of just "bar.h" - # Only do this check if the included header follows google naming - # conventions. If not, assume that it's a 3rd party API that - # requires special include conventions. - # - # We also make an exception for Lua headers, which follow google - # naming convention but not the include convention. - match = Match(r'#include\s*"([^/]+\.h)"', line) - if match and not _THIRD_PARTY_HEADERS_PATTERN.match(match.group(1)): - error(filename, linenum, 'build/include', 4, - 'Include the directory when naming .h files') - - # we shouldn't include a file more than once. actually, there are a - # handful of instances where doing so is okay, but in general it's - # not. - match = _RE_PATTERN_INCLUDE.search(line) - if match: - include = match.group(2) - is_system = (match.group(1) == '<') - duplicate_line = include_state.FindHeader(include) - if duplicate_line >= 0: - error(filename, linenum, 'build/include', 4, - '"%s" already included at %s:%s' % - (include, filename, duplicate_line)) - elif (include.endswith('.cc') and - os.path.dirname(fileinfo.RepositoryName()) != os.path.dirname(include)): - error(filename, linenum, 'build/include', 4, - 'Do not include .cc files from other packages') - elif not _THIRD_PARTY_HEADERS_PATTERN.match(include): - include_state.include_list[-1].append((include, linenum)) - - # We want to ensure that headers appear in the right order: - # 1) for foo.cc, foo.h (preferred location) - # 2) c system files - # 3) cpp system files - # 4) for foo.cc, foo.h (deprecated location) - # 5) other google headers - # - # We classify each include statement as one of those 5 types - # using a number of techniques. The include_state object keeps - # track of the highest type seen, and complains if we see a - # lower type after that. - error_message = include_state.CheckNextIncludeOrder( - _ClassifyInclude(fileinfo, include, is_system)) - if error_message: - error(filename, linenum, 'build/include_order', 4, - '%s. Should be: %s.h, c system, c++ system, other.' % - (error_message, fileinfo.BaseName())) - canonical_include = include_state.CanonicalizeAlphabeticalOrder(include) - if not include_state.IsInAlphabeticalOrder( - clean_lines, linenum, canonical_include): - error(filename, linenum, 'build/include_alpha', 4, - 'Include "%s" not in alphabetical order' % include) - include_state.SetLastHeader(canonical_include) - - - -def _GetTextInside(text, start_pattern): - r"""Retrieves all the text between matching open and close parentheses. - - Given a string of lines and a regular expression string, retrieve all the text - following the expression and between opening punctuation symbols like - (, [, or {, and the matching close-punctuation symbol. This properly nested - occurrences of the punctuations, so for the text like - printf(a(), b(c())); - a call to _GetTextInside(text, r'printf\(') will return 'a(), b(c())'. - start_pattern must match string having an open punctuation symbol at the end. - - Args: - text: The lines to extract text. Its comments and strings must be elided. - It can be single line and can span multiple lines. - start_pattern: The regexp string indicating where to start extracting - the text. - Returns: - The extracted text. - None if either the opening string or ending punctuation could not be found. - """ - # TODO(unknown): Audit cpplint.py to see what places could be profitably - # rewritten to use _GetTextInside (and use inferior regexp matching today). - - # Give opening punctuations to get the matching close-punctuations. - matching_punctuation = {'(': ')', '{': '}', '[': ']'} - closing_punctuation = set(matching_punctuation.itervalues()) - - # Find the position to start extracting text. - match = re.search(start_pattern, text, re.M) - if not match: # start_pattern not found in text. - return None - start_position = match.end(0) - - assert start_position > 0, ( - 'start_pattern must ends with an opening punctuation.') - assert text[start_position - 1] in matching_punctuation, ( - 'start_pattern must ends with an opening punctuation.') - # Stack of closing punctuations we expect to have in text after position. - punctuation_stack = [matching_punctuation[text[start_position - 1]]] - position = start_position - while punctuation_stack and position < len(text): - if text[position] == punctuation_stack[-1]: - punctuation_stack.pop() - elif text[position] in closing_punctuation: - # A closing punctuation without matching opening punctuations. - return None - elif text[position] in matching_punctuation: - punctuation_stack.append(matching_punctuation[text[position]]) - position += 1 - if punctuation_stack: - # Opening punctuations left without matching close-punctuations. - return None - # punctuations match. - return text[start_position:position - 1] - - -# Patterns for matching call-by-reference parameters. -# -# Supports nested templates up to 2 levels deep using this messy pattern: -# < (?: < (?: < [^<>]* -# > -# | [^<>] )* -# > -# | [^<>] )* -# > -_RE_PATTERN_IDENT = r'[_a-zA-Z]\w*' # =~ [[:alpha:]][[:alnum:]]* -_RE_PATTERN_TYPE = ( - r'(?:const\s+)?(?:typename\s+|class\s+|struct\s+|union\s+|enum\s+)?' - r'(?:\w|' - r'\s*<(?:<(?:<[^<>]*>|[^<>])*>|[^<>])*>|' - r'::)+') -# A call-by-reference parameter ends with '& identifier'. -_RE_PATTERN_REF_PARAM = re.compile( - r'(' + _RE_PATTERN_TYPE + r'(?:\s*(?:\bconst\b|[*]))*\s*' - r'&\s*' + _RE_PATTERN_IDENT + r')\s*(?:=[^,()]+)?[,)]') -# A call-by-const-reference parameter either ends with 'const& identifier' -# or looks like 'const type& identifier' when 'type' is atomic. -_RE_PATTERN_CONST_REF_PARAM = ( - r'(?:.*\s*\bconst\s*&\s*' + _RE_PATTERN_IDENT + - r'|const\s+' + _RE_PATTERN_TYPE + r'\s*&\s*' + _RE_PATTERN_IDENT + r')') -# Stream types. -_RE_PATTERN_REF_STREAM_PARAM = ( - r'(?:.*stream\s*&\s*' + _RE_PATTERN_IDENT + r')') - - -def CheckLanguage(filename, clean_lines, linenum, file_extension, - include_state, nesting_state, error): - """Checks rules from the 'C++ language rules' section of cppguide.html. - - Some of these rules are hard to test (function overloading, using - uint32 inappropriately), but we do the best we can. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - file_extension: The extension (without the dot) of the filename. - include_state: An _IncludeState instance in which the headers are inserted. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: The function to call with any errors found. - """ - # If the line is empty or consists of entirely a comment, no need to - # check it. - line = clean_lines.elided[linenum] - if not line: - return - - match = _RE_PATTERN_INCLUDE.search(line) - if match: - CheckIncludeLine(filename, clean_lines, linenum, include_state, error) - return - - # Reset include state across preprocessor directives. This is meant - # to silence warnings for conditional includes. - match = Match(r'^\s*#\s*(if|ifdef|ifndef|elif|else|endif)\b', line) - if match: - include_state.ResetSection(match.group(1)) - - # Make Windows paths like Unix. - fullname = os.path.abspath(filename).replace('\\', '/') - - # Perform other checks now that we are sure that this is not an include line - CheckCasts(filename, clean_lines, linenum, error) - CheckGlobalStatic(filename, clean_lines, linenum, error) - CheckPrintf(filename, clean_lines, linenum, error) - - if IsHeaderExtension(file_extension): - # TODO(unknown): check that 1-arg constructors are explicit. - # How to tell it's a constructor? - # (handled in CheckForNonStandardConstructs for now) - # TODO(unknown): check that classes declare or disable copy/assign - # (level 1 error) - pass - - # Check if people are using the verboten C basic types. The only exception - # we regularly allow is "unsigned short port" for port. - if Search(r'\bshort port\b', line): - if not Search(r'\bunsigned short port\b', line): - error(filename, linenum, 'runtime/int', 4, - 'Use "unsigned short" for ports, not "short"') - else: - match = Search(r'\b(short|long(?! +double)|long long)\b', line) - if match: - error(filename, linenum, 'runtime/int', 4, - 'Use int16/int64/etc, rather than the C type %s' % match.group(1)) - - # Check if some verboten operator overloading is going on - # TODO(unknown): catch out-of-line unary operator&: - # class X {}; - # int operator&(const X& x) { return 42; } // unary operator& - # The trick is it's hard to tell apart from binary operator&: - # class Y { int operator&(const Y& x) { return 23; } }; // binary operator& - if Search(r'\boperator\s*&\s*\(\s*\)', line): - error(filename, linenum, 'runtime/operator', 4, - 'Unary operator& is dangerous. Do not use it.') - - # Check for suspicious usage of "if" like - # } if (a == b) { - if Search(r'\}\s*if\s*\(', line): - error(filename, linenum, 'readability/braces', 4, - 'Did you mean "else if"? If not, start a new line for "if".') - - # Check for potential format string bugs like printf(foo). - # We constrain the pattern not to pick things like DocidForPrintf(foo). - # Not perfect but it can catch printf(foo.c_str()) and printf(foo->c_str()) - # TODO(unknown): Catch the following case. Need to change the calling - # convention of the whole function to process multiple line to handle it. - # printf( - # boy_this_is_a_really_long_variable_that_cannot_fit_on_the_prev_line); - printf_args = _GetTextInside(line, r'(?i)\b(string)?printf\s*\(') - if printf_args: - match = Match(r'([\w.\->()]+)$', printf_args) - if match and match.group(1) != '__VA_ARGS__': - function_name = re.search(r'\b((?:string)?printf)\s*\(', - line, re.I).group(1) - error(filename, linenum, 'runtime/printf', 4, - 'Potential format string bug. Do %s("%%s", %s) instead.' - % (function_name, match.group(1))) - - # Check for potential memset bugs like memset(buf, sizeof(buf), 0). - match = Search(r'memset\s*\(([^,]*),\s*([^,]*),\s*0\s*\)', line) - if match and not Match(r"^''|-?[0-9]+|0x[0-9A-Fa-f]$", match.group(2)): - error(filename, linenum, 'runtime/memset', 4, - 'Did you mean "memset(%s, 0, %s)"?' - % (match.group(1), match.group(2))) - - # if Search(r'\busing namespace\b', line): - # error(filename, linenum, 'build/namespaces', 5, - # 'Do not use namespace using-directives. ' - # 'Use using-declarations instead.') - - # Detect variable-length arrays. - match = Match(r'\s*(.+::)?(\w+) [a-z]\w*\[(.+)];', line) - if (match and match.group(2) != 'return' and match.group(2) != 'delete' and - match.group(3).find(']') == -1): - # Split the size using space and arithmetic operators as delimiters. - # If any of the resulting tokens are not compile time constants then - # report the error. - tokens = re.split(r'\s|\+|\-|\*|\/|<<|>>]', match.group(3)) - is_const = True - skip_next = False - for tok in tokens: - if skip_next: - skip_next = False - continue - - if Search(r'sizeof\(.+\)', tok): continue - if Search(r'arraysize\(\w+\)', tok): continue - - tok = tok.lstrip('(') - tok = tok.rstrip(')') - if not tok: continue - if Match(r'\d+', tok): continue - if Match(r'0[xX][0-9a-fA-F]+', tok): continue - if Match(r'k[A-Z0-9]\w*', tok): continue - if Match(r'(.+::)?k[A-Z0-9]\w*', tok): continue - if Match(r'(.+::)?[A-Z][A-Z0-9_]*', tok): continue - # A catch all for tricky sizeof cases, including 'sizeof expression', - # 'sizeof(*type)', 'sizeof(const type)', 'sizeof(struct StructName)' - # requires skipping the next token because we split on ' ' and '*'. - if tok.startswith('sizeof'): - skip_next = True - continue - is_const = False - break - if not is_const: - error(filename, linenum, 'runtime/arrays', 1, - 'Do not use variable-length arrays. Use an appropriately named ' - "('k' followed by CamelCase) compile-time constant for the size.") - - # Check for use of unnamed namespaces in header files. Registration - # macros are typically OK, so we allow use of "namespace {" on lines - # that end with backslashes. - if (IsHeaderExtension(file_extension) - and Search(r'\bnamespace\s*{', line) - and line[-1] != '\\'): - error(filename, linenum, 'build/namespaces', 4, - 'Do not use unnamed namespaces in header files. See ' - 'https://google-styleguide.googlecode.com/svn/trunk/cppguide.xml#Namespaces' - ' for more information.') - - -def CheckGlobalStatic(filename, clean_lines, linenum, error): - """Check for unsafe global or static objects. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Match two lines at a time to support multiline declarations - if linenum + 1 < clean_lines.NumLines() and not Search(r'[;({]', line): - line += clean_lines.elided[linenum + 1].strip() - - # Check for people declaring static/global STL strings at the top level. - # This is dangerous because the C++ language does not guarantee that - # globals with constructors are initialized before the first access, and - # also because globals can be destroyed when some threads are still running. - # TODO(unknown): Generalize this to also find static unique_ptr instances. - # TODO(unknown): File bugs for clang-tidy to find these. - match = Match( - r'((?:|static +)(?:|const +))(?::*std::)?string( +const)? +' - r'([a-zA-Z0-9_:]+)\b(.*)', - line) - - # Remove false positives: - # - String pointers (as opposed to values). - # string *pointer - # const string *pointer - # string const *pointer - # string *const pointer - # - # - Functions and template specializations. - # string Function(... - # string Class::Method(... - # - # - Operators. These are matched separately because operator names - # cross non-word boundaries, and trying to match both operators - # and functions at the same time would decrease accuracy of - # matching identifiers. - # string Class::operator*() - if (match and - not Search(r'\bstring\b(\s+const)?\s*[\*\&]\s*(const\s+)?\w', line) and - not Search(r'\boperator\W', line) and - not Match(r'\s*(<.*>)?(::[a-zA-Z0-9_]+)*\s*\(([^"]|$)', match.group(4))): - if Search(r'\bconst\b', line): - error(filename, linenum, 'runtime/string', 4, - 'For a static/global string constant, use a C style string ' - 'instead: "%schar%s %s[]".' % - (match.group(1), match.group(2) or '', match.group(3))) - else: - error(filename, linenum, 'runtime/string', 4, - 'Static/global string variables are not permitted.') - - if (Search(r'\b([A-Za-z0-9_]*_)\(\1\)', line) or - Search(r'\b([A-Za-z0-9_]*_)\(CHECK_NOTNULL\(\1\)\)', line)): - error(filename, linenum, 'runtime/init', 4, - 'You seem to be initializing a member variable with itself.') - - -def CheckPrintf(filename, clean_lines, linenum, error): - """Check for printf related issues. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # When snprintf is used, the second argument shouldn't be a literal. - match = Search(r'snprintf\s*\(([^,]*),\s*([0-9]*)\s*,', line) - if match and match.group(2) != '0': - # If 2nd arg is zero, snprintf is used to calculate size. - error(filename, linenum, 'runtime/printf', 3, - 'If you can, use sizeof(%s) instead of %s as the 2nd arg ' - 'to snprintf.' % (match.group(1), match.group(2))) - - # Check if some verboten C functions are being used. - if Search(r'\bsprintf\s*\(', line): - error(filename, linenum, 'runtime/printf', 5, - 'Never use sprintf. Use snprintf instead.') - match = Search(r'\b(strcpy|strcat)\s*\(', line) - if match: - error(filename, linenum, 'runtime/printf', 4, - 'Almost always, snprintf is better than %s' % match.group(1)) - - -def IsDerivedFunction(clean_lines, linenum): - """Check if current line contains an inherited function. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - Returns: - True if current line contains a function with "override" - virt-specifier. - """ - # Scan back a few lines for start of current function - for i in xrange(linenum, max(-1, linenum - 10), -1): - match = Match(r'^([^()]*\w+)\(', clean_lines.elided[i]) - if match: - # Look for "override" after the matching closing parenthesis - line, _, closing_paren = CloseExpression( - clean_lines, i, len(match.group(1))) - return (closing_paren >= 0 and - Search(r'\boverride\b', line[closing_paren:])) - return False - - -def IsOutOfLineMethodDefinition(clean_lines, linenum): - """Check if current line contains an out-of-line method definition. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - Returns: - True if current line contains an out-of-line method definition. - """ - # Scan back a few lines for start of current function - for i in xrange(linenum, max(-1, linenum - 10), -1): - if Match(r'^([^()]*\w+)\(', clean_lines.elided[i]): - return Match(r'^[^()]*\w+::\w+\(', clean_lines.elided[i]) is not None - return False - - -def IsInitializerList(clean_lines, linenum): - """Check if current line is inside constructor initializer list. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - Returns: - True if current line appears to be inside constructor initializer - list, False otherwise. - """ - for i in xrange(linenum, 1, -1): - line = clean_lines.elided[i] - if i == linenum: - remove_function_body = Match(r'^(.*)\{\s*$', line) - if remove_function_body: - line = remove_function_body.group(1) - - if Search(r'\s:\s*\w+[({]', line): - # A lone colon tend to indicate the start of a constructor - # initializer list. It could also be a ternary operator, which - # also tend to appear in constructor initializer lists as - # opposed to parameter lists. - return True - if Search(r'\}\s*,\s*$', line): - # A closing brace followed by a comma is probably the end of a - # brace-initialized member in constructor initializer list. - return True - if Search(r'[{};]\s*$', line): - # Found one of the following: - # - A closing brace or semicolon, probably the end of the previous - # function. - # - An opening brace, probably the start of current class or namespace. - # - # Current line is probably not inside an initializer list since - # we saw one of those things without seeing the starting colon. - return False - - # Got to the beginning of the file without seeing the start of - # constructor initializer list. - return False - - -def CheckForNonConstReference(filename, clean_lines, linenum, - nesting_state, error): - """Check for non-const references. - - Separate from CheckLanguage since it scans backwards from current - line, instead of scanning forward. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: The function to call with any errors found. - """ - # Do nothing if there is no '&' on current line. - line = clean_lines.elided[linenum] - if '&' not in line: - return - - # If a function is inherited, current function doesn't have much of - # a choice, so any non-const references should not be blamed on - # derived function. - if IsDerivedFunction(clean_lines, linenum): - return - - # Don't warn on out-of-line method definitions, as we would warn on the - # in-line declaration, if it isn't marked with 'override'. - if IsOutOfLineMethodDefinition(clean_lines, linenum): - return - - # Long type names may be broken across multiple lines, usually in one - # of these forms: - # LongType - # ::LongTypeContinued &identifier - # LongType:: - # LongTypeContinued &identifier - # LongType< - # ...>::LongTypeContinued &identifier - # - # If we detected a type split across two lines, join the previous - # line to current line so that we can match const references - # accordingly. - # - # Note that this only scans back one line, since scanning back - # arbitrary number of lines would be expensive. If you have a type - # that spans more than 2 lines, please use a typedef. - if linenum > 1: - previous = None - if Match(r'\s*::(?:[\w<>]|::)+\s*&\s*\S', line): - # previous_line\n + ::current_line - previous = Search(r'\b((?:const\s*)?(?:[\w<>]|::)+[\w<>])\s*$', - clean_lines.elided[linenum - 1]) - elif Match(r'\s*[a-zA-Z_]([\w<>]|::)+\s*&\s*\S', line): - # previous_line::\n + current_line - previous = Search(r'\b((?:const\s*)?(?:[\w<>]|::)+::)\s*$', - clean_lines.elided[linenum - 1]) - if previous: - line = previous.group(1) + line.lstrip() - else: - # Check for templated parameter that is split across multiple lines - endpos = line.rfind('>') - if endpos > -1: - (_, startline, startpos) = ReverseCloseExpression( - clean_lines, linenum, endpos) - if startpos > -1 and startline < linenum: - # Found the matching < on an earlier line, collect all - # pieces up to current line. - line = '' - for i in xrange(startline, linenum + 1): - line += clean_lines.elided[i].strip() - - # Check for non-const references in function parameters. A single '&' may - # found in the following places: - # inside expression: binary & for bitwise AND - # inside expression: unary & for taking the address of something - # inside declarators: reference parameter - # We will exclude the first two cases by checking that we are not inside a - # function body, including one that was just introduced by a trailing '{'. - # TODO(unknown): Doesn't account for 'catch(Exception& e)' [rare]. - if (nesting_state.previous_stack_top and - not (isinstance(nesting_state.previous_stack_top, _ClassInfo) or - isinstance(nesting_state.previous_stack_top, _NamespaceInfo))): - # Not at toplevel, not within a class, and not within a namespace - return - - # Avoid initializer lists. We only need to scan back from the - # current line for something that starts with ':'. - # - # We don't need to check the current line, since the '&' would - # appear inside the second set of parentheses on the current line as - # opposed to the first set. - if linenum > 0: - for i in xrange(linenum - 1, max(0, linenum - 10), -1): - previous_line = clean_lines.elided[i] - if not Search(r'[),]\s*$', previous_line): - break - if Match(r'^\s*:\s+\S', previous_line): - return - - # Avoid preprocessors - if Search(r'\\\s*$', line): - return - - # Avoid constructor initializer lists - if IsInitializerList(clean_lines, linenum): - return - - # We allow non-const references in a few standard places, like functions - # called "swap()" or iostream operators like "<<" or ">>". Do not check - # those function parameters. - # - # We also accept & in static_assert, which looks like a function but - # it's actually a declaration expression. - whitelisted_functions = (r'(?:[sS]wap(?:<\w:+>)?|' - r'operator\s*[<>][<>]|' - r'static_assert|COMPILE_ASSERT' - r')\s*\(') - if Search(whitelisted_functions, line): - return - elif not Search(r'\S+\([^)]*$', line): - # Don't see a whitelisted function on this line. Actually we - # didn't see any function name on this line, so this is likely a - # multi-line parameter list. Try a bit harder to catch this case. - for i in xrange(2): - if (linenum > i and - Search(whitelisted_functions, clean_lines.elided[linenum - i - 1])): - return - - decls = ReplaceAll(r'{[^}]*}', ' ', line) # exclude function body - # for parameter in re.findall(_RE_PATTERN_REF_PARAM, decls): - # if (not Match(_RE_PATTERN_CONST_REF_PARAM, parameter) and - # not Match(_RE_PATTERN_REF_STREAM_PARAM, parameter)): - # error(filename, linenum, 'runtime/references', 2, - # 'Is this a non-const reference? ' - # 'If so, make const or use a pointer: ' + - # ReplaceAll(' *<', '<', parameter)) - - -def CheckCasts(filename, clean_lines, linenum, error): - """Various cast related checks. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - # Check to see if they're using an conversion function cast. - # I just try to capture the most common basic types, though there are more. - # Parameterless conversion functions, such as bool(), are allowed as they are - # probably a member operator declaration or default constructor. - match = Search( - r'(\bnew\s+(?:const\s+)?|\S<\s*(?:const\s+)?)?\b' - r'(int|float|double|bool|char|int32|uint32|int64|uint64)' - r'(\([^)].*)', line) - expecting_function = ExpectingFunctionArgs(clean_lines, linenum) - if match and not expecting_function: - matched_type = match.group(2) - - # matched_new_or_template is used to silence two false positives: - # - New operators - # - Template arguments with function types - # - # For template arguments, we match on types immediately following - # an opening bracket without any spaces. This is a fast way to - # silence the common case where the function type is the first - # template argument. False negative with less-than comparison is - # avoided because those operators are usually followed by a space. - # - # function // bracket + no space = false positive - # value < double(42) // bracket + space = true positive - matched_new_or_template = match.group(1) - - # Avoid arrays by looking for brackets that come after the closing - # parenthesis. - if Match(r'\([^()]+\)\s*\[', match.group(3)): - return - - # Other things to ignore: - # - Function pointers - # - Casts to pointer types - # - Placement new - # - Alias declarations - matched_funcptr = match.group(3) - if (matched_new_or_template is None and - not (matched_funcptr and - (Match(r'\((?:[^() ]+::\s*\*\s*)?[^() ]+\)\s*\(', - matched_funcptr) or - matched_funcptr.startswith('(*)'))) and - not Match(r'\s*using\s+\S+\s*=\s*' + matched_type, line) and - not Search(r'new\(\S+\)\s*' + matched_type, line)): - error(filename, linenum, 'readability/casting', 4, - 'Using deprecated casting style. ' - 'Use static_cast<%s>(...) instead' % - matched_type) - - if not expecting_function: - CheckCStyleCast(filename, clean_lines, linenum, 'static_cast', - r'\((int|float|double|bool|char|u?int(16|32|64))\)', error) - - # This doesn't catch all cases. Consider (const char * const)"hello". - # - # (char *) "foo" should always be a const_cast (reinterpret_cast won't - # compile). - if CheckCStyleCast(filename, clean_lines, linenum, 'const_cast', - r'\((char\s?\*+\s?)\)\s*"', error): - pass - else: - # Check pointer casts for other than string constants - CheckCStyleCast(filename, clean_lines, linenum, 'reinterpret_cast', - r'\((\w+\s?\*+\s?)\)', error) - - # In addition, we look for people taking the address of a cast. This - # is dangerous -- casts can assign to temporaries, so the pointer doesn't - # point where you think. - # - # Some non-identifier character is required before the '&' for the - # expression to be recognized as a cast. These are casts: - # expression = &static_cast(temporary()); - # function(&(int*)(temporary())); - # - # This is not a cast: - # reference_type&(int* function_param); - match = Search( - r'(?:[^\w]&\(([^)*][^)]*)\)[\w(])|' - r'(?:[^\w]&(static|dynamic|down|reinterpret)_cast\b)', line) - if match: - # Try a better error message when the & is bound to something - # dereferenced by the casted pointer, as opposed to the casted - # pointer itself. - parenthesis_error = False - match = Match(r'^(.*&(?:static|dynamic|down|reinterpret)_cast\b)<', line) - if match: - _, y1, x1 = CloseExpression(clean_lines, linenum, len(match.group(1))) - if x1 >= 0 and clean_lines.elided[y1][x1] == '(': - _, y2, x2 = CloseExpression(clean_lines, y1, x1) - if x2 >= 0: - extended_line = clean_lines.elided[y2][x2:] - if y2 < clean_lines.NumLines() - 1: - extended_line += clean_lines.elided[y2 + 1] - if Match(r'\s*(?:->|\[)', extended_line): - parenthesis_error = True - - if parenthesis_error: - error(filename, linenum, 'readability/casting', 4, - ('Are you taking an address of something dereferenced ' - 'from a cast? Wrapping the dereferenced expression in ' - 'parentheses will make the binding more obvious')) - else: - error(filename, linenum, 'runtime/casting', 4, - ('Are you taking an address of a cast? ' - 'This is dangerous: could be a temp var. ' - 'Take the address before doing the cast, rather than after')) - - -def CheckCStyleCast(filename, clean_lines, linenum, cast_type, pattern, error): - """Checks for a C-style cast by looking for the pattern. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - cast_type: The string for the C++ cast to recommend. This is either - reinterpret_cast, static_cast, or const_cast, depending. - pattern: The regular expression used to find C-style casts. - error: The function to call with any errors found. - - Returns: - True if an error was emitted. - False otherwise. - """ - line = clean_lines.elided[linenum] - match = Search(pattern, line) - if not match: - return False - - # Exclude lines with keywords that tend to look like casts - context = line[0:match.start(1) - 1] - if Match(r'^\s*(private|public):.*$', context): - return False - if Match(r'.*\b(?:sizeof|alignof|alignas|[_A-Z][_A-Z0-9]*)\s*$', context): - return False - - # Try expanding current context to see if we one level of - # parentheses inside a macro. - if linenum > 0: - for i in xrange(linenum - 1, max(0, linenum - 5), -1): - context = clean_lines.elided[i] + context - if Match(r'.*\b[_A-Z][_A-Z0-9]*\s*\((?:\([^()]*\)|[^()])*$', context): - return False - - # operator++(int) and operator--(int) - if context.endswith(' operator++') or context.endswith(' operator--'): - return False - - # A single unnamed argument for a function tends to look like old style cast. - # If we see those, don't issue warnings for deprecated casts. - remainder = line[match.end(0):] - if Match(r'^\s*(?:;|const\b|throw\b|final\b|override\b|[=>{),]|->)', - remainder): - return False - - # At this point, all that should be left is actual casts. - error(filename, linenum, 'readability/casting', 4, - 'Using C-style cast. Use %s<%s>(...) instead' % - (cast_type, match.group(1))) - - return True - - -def ExpectingFunctionArgs(clean_lines, linenum): - """Checks whether where function type arguments are expected. - - Args: - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - - Returns: - True if the line at 'linenum' is inside something that expects arguments - of function types. - """ - line = clean_lines.elided[linenum] - return (Match(r'^\s*MOCK_(CONST_)?METHOD\d+(_T)?\(', line) or - (linenum >= 2 and - (Match(r'^\s*MOCK_(?:CONST_)?METHOD\d+(?:_T)?\((?:\S+,)?\s*$', - clean_lines.elided[linenum - 1]) or - Match(r'^\s*MOCK_(?:CONST_)?METHOD\d+(?:_T)?\(\s*$', - clean_lines.elided[linenum - 2]) or - Search(r'\bstd::m?function\s*\<\s*$', - clean_lines.elided[linenum - 1])))) - - -_HEADERS_CONTAINING_TEMPLATES = ( - ('', ('deque',)), - ('', ('unary_function', 'binary_function', - 'plus', 'minus', 'multiplies', 'divides', 'modulus', - 'negate', - 'equal_to', 'not_equal_to', 'greater', 'less', - 'greater_equal', 'less_equal', - 'logical_and', 'logical_or', 'logical_not', - 'unary_negate', 'not1', 'binary_negate', 'not2', - 'bind1st', 'bind2nd', - 'pointer_to_unary_function', - 'pointer_to_binary_function', - 'ptr_fun', - 'mem_fun_t', 'mem_fun', 'mem_fun1_t', 'mem_fun1_ref_t', - 'mem_fun_ref_t', - 'const_mem_fun_t', 'const_mem_fun1_t', - 'const_mem_fun_ref_t', 'const_mem_fun1_ref_t', - 'mem_fun_ref', - )), - ('', ('numeric_limits',)), - ('', ('list',)), - ('', ('map', 'multimap',)), - ('', ('allocator', 'make_shared', 'make_unique', 'shared_ptr', - 'unique_ptr', 'weak_ptr')), - ('', ('queue', 'priority_queue',)), - ('', ('set', 'multiset',)), - ('', ('stack',)), - ('', ('char_traits', 'basic_string',)), - ('', ('tuple',)), - ('', ('unordered_map', 'unordered_multimap')), - ('', ('unordered_set', 'unordered_multiset')), - ('', ('pair',)), - ('', ('vector',)), - - # gcc extensions. - # Note: std::hash is their hash, ::hash is our hash - ('', ('hash_map', 'hash_multimap',)), - ('', ('hash_set', 'hash_multiset',)), - ('', ('slist',)), - ) - -_HEADERS_MAYBE_TEMPLATES = ( - ('', ('copy', 'max', 'min', 'min_element', 'sort', - 'transform', - )), - ('', ('forward', 'make_pair', 'move', 'swap')), - ) - -_RE_PATTERN_STRING = re.compile(r'\bstring\b') - -_re_pattern_headers_maybe_templates = [] -for _header, _templates in _HEADERS_MAYBE_TEMPLATES: - for _template in _templates: - # Match max(..., ...), max(..., ...), but not foo->max, foo.max or - # type::max(). - _re_pattern_headers_maybe_templates.append( - (re.compile(r'[^>.]\b' + _template + r'(<.*?>)?\([^\)]'), - _template, - _header)) - -# Other scripts may reach in and modify this pattern. -_re_pattern_templates = [] -for _header, _templates in _HEADERS_CONTAINING_TEMPLATES: - for _template in _templates: - _re_pattern_templates.append( - (re.compile(r'(\<|\b)' + _template + r'\s*\<'), - _template + '<>', - _header)) - - -def FilesBelongToSameModule(filename_cc, filename_h): - """Check if these two filenames belong to the same module. - - The concept of a 'module' here is a as follows: - foo.h, foo-inl.h, foo.cc, foo_test.cc and foo_unittest.cc belong to the - same 'module' if they are in the same directory. - some/path/public/xyzzy and some/path/internal/xyzzy are also considered - to belong to the same module here. - - If the filename_cc contains a longer path than the filename_h, for example, - '/absolute/path/to/base/sysinfo.cc', and this file would include - 'base/sysinfo.h', this function also produces the prefix needed to open the - header. This is used by the caller of this function to more robustly open the - header file. We don't have access to the real include paths in this context, - so we need this guesswork here. - - Known bugs: tools/base/bar.cc and base/bar.h belong to the same module - according to this implementation. Because of this, this function gives - some false positives. This should be sufficiently rare in practice. - - Args: - filename_cc: is the path for the .cc file - filename_h: is the path for the header path - - Returns: - Tuple with a bool and a string: - bool: True if filename_cc and filename_h belong to the same module. - string: the additional prefix needed to open the header file. - """ - - fileinfo = FileInfo(filename_cc) - if not fileinfo.IsSource(): - return (False, '') - filename_cc = filename_cc[:-len(fileinfo.Extension())] - matched_test_suffix = Search(_TEST_FILE_SUFFIX, fileinfo.BaseName()) - if matched_test_suffix: - filename_cc = filename_cc[:-len(matched_test_suffix.group(1))] - filename_cc = filename_cc.replace('/public/', '/') - filename_cc = filename_cc.replace('/internal/', '/') - - if not filename_h.endswith('.h'): - return (False, '') - filename_h = filename_h[:-len('.h')] - if filename_h.endswith('-inl'): - filename_h = filename_h[:-len('-inl')] - filename_h = filename_h.replace('/public/', '/') - filename_h = filename_h.replace('/internal/', '/') - - files_belong_to_same_module = filename_cc.endswith(filename_h) - common_path = '' - if files_belong_to_same_module: - common_path = filename_cc[:-len(filename_h)] - return files_belong_to_same_module, common_path - - -def UpdateIncludeState(filename, include_dict, io=codecs): - """Fill up the include_dict with new includes found from the file. - - Args: - filename: the name of the header to read. - include_dict: a dictionary in which the headers are inserted. - io: The io factory to use to read the file. Provided for testability. - - Returns: - True if a header was successfully added. False otherwise. - """ - headerfile = None - try: - headerfile = io.open(filename, 'r', 'utf8', 'replace') - except IOError: - return False - linenum = 0 - for line in headerfile: - linenum += 1 - clean_line = CleanseComments(line) - match = _RE_PATTERN_INCLUDE.search(clean_line) - if match: - include = match.group(2) - include_dict.setdefault(include, linenum) - return True - - -def CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error, - io=codecs): - """Reports for missing stl includes. - - This function will output warnings to make sure you are including the headers - necessary for the stl containers and functions that you use. We only give one - reason to include a header. For example, if you use both equal_to<> and - less<> in a .h file, only one (the latter in the file) of these will be - reported as a reason to include the . - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - include_state: An _IncludeState instance. - error: The function to call with any errors found. - io: The IO factory to use to read the header file. Provided for unittest - injection. - """ - required = {} # A map of header name to linenumber and the template entity. - # Example of required: { '': (1219, 'less<>') } - - for linenum in xrange(clean_lines.NumLines()): - line = clean_lines.elided[linenum] - if not line or line[0] == '#': - continue - - # String is special -- it is a non-templatized type in STL. - matched = _RE_PATTERN_STRING.search(line) - if matched: - # Don't warn about strings in non-STL namespaces: - # (We check only the first match per line; good enough.) - prefix = line[:matched.start()] - if prefix.endswith('std::') or not prefix.endswith('::'): - required[''] = (linenum, 'string') - - for pattern, template, header in _re_pattern_headers_maybe_templates: - if pattern.search(line): - required[header] = (linenum, template) - - # The following function is just a speed up, no semantics are changed. - if not '<' in line: # Reduces the cpu time usage by skipping lines. - continue - - for pattern, template, header in _re_pattern_templates: - matched = pattern.search(line) - if matched: - # Don't warn about IWYU in non-STL namespaces: - # (We check only the first match per line; good enough.) - prefix = line[:matched.start()] - if prefix.endswith('std::') or not prefix.endswith('::'): - required[header] = (linenum, template) - - # The policy is that if you #include something in foo.h you don't need to - # include it again in foo.cc. Here, we will look at possible includes. - # Let's flatten the include_state include_list and copy it into a dictionary. - include_dict = dict([item for sublist in include_state.include_list - for item in sublist]) - - # Did we find the header for this file (if any) and successfully load it? - header_found = False - - # Use the absolute path so that matching works properly. - abs_filename = FileInfo(filename).FullName() - - # For Emacs's flymake. - # If cpplint is invoked from Emacs's flymake, a temporary file is generated - # by flymake and that file name might end with '_flymake.cc'. In that case, - # restore original file name here so that the corresponding header file can be - # found. - # e.g. If the file name is 'foo_flymake.cc', we should search for 'foo.h' - # instead of 'foo_flymake.h' - abs_filename = re.sub(r'_flymake\.cc$', '.cc', abs_filename) - - # include_dict is modified during iteration, so we iterate over a copy of - # the keys. - header_keys = include_dict.keys() - for header in header_keys: - (same_module, common_path) = FilesBelongToSameModule(abs_filename, header) - fullpath = common_path + header - if same_module and UpdateIncludeState(fullpath, include_dict, io): - header_found = True - - # If we can't find the header file for a .cc, assume it's because we don't - # know where to look. In that case we'll give up as we're not sure they - # didn't include it in the .h file. - # TODO(unknown): Do a better job of finding .h files so we are confident that - # not having the .h file means there isn't one. - if filename.endswith('.cc') and not header_found: - return - - # All the lines have been processed, report the errors found. - for required_header_unstripped in required: - template = required[required_header_unstripped][1] - if required_header_unstripped.strip('<>"') not in include_dict: - error(filename, required[required_header_unstripped][0], - 'build/include_what_you_use', 4, - 'Add #include ' + required_header_unstripped + ' for ' + template) - - -_RE_PATTERN_EXPLICIT_MAKEPAIR = re.compile(r'\bmake_pair\s*<') - - -def CheckMakePairUsesDeduction(filename, clean_lines, linenum, error): - """Check that make_pair's template arguments are deduced. - - G++ 4.6 in C++11 mode fails badly if make_pair's template arguments are - specified explicitly, and such use isn't intended in any case. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - match = _RE_PATTERN_EXPLICIT_MAKEPAIR.search(line) - if match: - error(filename, linenum, 'build/explicit_make_pair', - 4, # 4 = high confidence - 'For C++11-compatibility, omit template arguments from make_pair' - ' OR use pair directly OR if appropriate, construct a pair directly') - - -def CheckRedundantVirtual(filename, clean_lines, linenum, error): - """Check if line contains a redundant "virtual" function-specifier. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - # Look for "virtual" on current line. - line = clean_lines.elided[linenum] - virtual = Match(r'^(.*)(\bvirtual\b)(.*)$', line) - if not virtual: return - - # Ignore "virtual" keywords that are near access-specifiers. These - # are only used in class base-specifier and do not apply to member - # functions. - if (Search(r'\b(public|protected|private)\s+$', virtual.group(1)) or - Match(r'^\s+(public|protected|private)\b', virtual.group(3))): - return - - # Ignore the "virtual" keyword from virtual base classes. Usually - # there is a column on the same line in these cases (virtual base - # classes are rare in google3 because multiple inheritance is rare). - if Match(r'^.*[^:]:[^:].*$', line): return - - # Look for the next opening parenthesis. This is the start of the - # parameter list (possibly on the next line shortly after virtual). - # TODO(unknown): doesn't work if there are virtual functions with - # decltype() or other things that use parentheses, but csearch suggests - # that this is rare. - end_col = -1 - end_line = -1 - start_col = len(virtual.group(2)) - for start_line in xrange(linenum, min(linenum + 3, clean_lines.NumLines())): - line = clean_lines.elided[start_line][start_col:] - parameter_list = Match(r'^([^(]*)\(', line) - if parameter_list: - # Match parentheses to find the end of the parameter list - (_, end_line, end_col) = CloseExpression( - clean_lines, start_line, start_col + len(parameter_list.group(1))) - break - start_col = 0 - - if end_col < 0: - return # Couldn't find end of parameter list, give up - - # Look for "override" or "final" after the parameter list - # (possibly on the next few lines). - for i in xrange(end_line, min(end_line + 3, clean_lines.NumLines())): - line = clean_lines.elided[i][end_col:] - match = Search(r'\b(override|final)\b', line) - if match: - error(filename, linenum, 'readability/inheritance', 4, - ('"virtual" is redundant since function is ' - 'already declared as "%s"' % match.group(1))) - - # Set end_col to check whole lines after we are done with the - # first line. - end_col = 0 - if Search(r'[^\w]\s*$', line): - break - - -def CheckRedundantOverrideOrFinal(filename, clean_lines, linenum, error): - """Check if line contains a redundant "override" or "final" virt-specifier. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - # Look for closing parenthesis nearby. We need one to confirm where - # the declarator ends and where the virt-specifier starts to avoid - # false positives. - line = clean_lines.elided[linenum] - declarator_end = line.rfind(')') - if declarator_end >= 0: - fragment = line[declarator_end:] - else: - if linenum > 1 and clean_lines.elided[linenum - 1].rfind(')') >= 0: - fragment = line - else: - return - - - - -# Returns true if we are at a new block, and it is directly -# inside of a namespace. -def IsBlockInNameSpace(nesting_state, is_forward_declaration): - """Checks that the new block is directly in a namespace. - - Args: - nesting_state: The _NestingState object that contains info about our state. - is_forward_declaration: If the class is a forward declared class. - Returns: - Whether or not the new block is directly in a namespace. - """ - if is_forward_declaration: - if len(nesting_state.stack) >= 1 and ( - isinstance(nesting_state.stack[-1], _NamespaceInfo)): - return True - else: - return False - - return (len(nesting_state.stack) > 1 and - nesting_state.stack[-1].check_namespace_indentation and - isinstance(nesting_state.stack[-2], _NamespaceInfo)) - - -def ShouldCheckNamespaceIndentation(nesting_state, is_namespace_indent_item, - raw_lines_no_comments, linenum): - """This method determines if we should apply our namespace indentation check. - - Args: - nesting_state: The current nesting state. - is_namespace_indent_item: If we just put a new class on the stack, True. - If the top of the stack is not a class, or we did not recently - add the class, False. - raw_lines_no_comments: The lines without the comments. - linenum: The current line number we are processing. - - Returns: - True if we should apply our namespace indentation check. Currently, it - only works for classes and namespaces inside of a namespace. - """ - - is_forward_declaration = IsForwardClassDeclaration(raw_lines_no_comments, - linenum) - - if not (is_namespace_indent_item or is_forward_declaration): - return False - - # If we are in a macro, we do not want to check the namespace indentation. - if IsMacroDefinition(raw_lines_no_comments, linenum): - return False - - return IsBlockInNameSpace(nesting_state, is_forward_declaration) - - -# Call this method if the line is directly inside of a namespace. -# If the line above is blank (excluding comments) or the start of -# an inner namespace, it cannot be indented. -def CheckItemIndentationInNamespace(filename, raw_lines_no_comments, linenum, - error): - line = raw_lines_no_comments[linenum] - # if Match(r'^\s+', line): - # error(filename, linenum, 'runtime/indentation_namespace', 4, - # 'Do not indent within a namespace') - - -def ProcessLine(filename, file_extension, clean_lines, line, - include_state, function_state, nesting_state, error, - extra_check_functions=[]): - """Processes a single line in the file. - - Args: - filename: Filename of the file that is being processed. - file_extension: The extension (dot not included) of the file. - clean_lines: An array of strings, each representing a line of the file, - with comments stripped. - line: Number of line being processed. - include_state: An _IncludeState instance in which the headers are inserted. - function_state: A _FunctionState instance which counts function lines, etc. - nesting_state: A NestingState instance which maintains information about - the current stack of nested blocks being parsed. - error: A callable to which errors are reported, which takes 4 arguments: - filename, line number, error level, and message - extra_check_functions: An array of additional check functions that will be - run on each source line. Each function takes 4 - arguments: filename, clean_lines, line, error - """ - raw_lines = clean_lines.raw_lines - ParseNolintSuppressions(filename, raw_lines[line], line, error) - nesting_state.Update(filename, clean_lines, line, error) - CheckForNamespaceIndentation(filename, nesting_state, clean_lines, line, - error) - if nesting_state.InAsmBlock(): return - CheckForFunctionLengths(filename, clean_lines, line, function_state, error) - CheckForMultilineCommentsAndStrings(filename, clean_lines, line, error) - CheckStyle(filename, clean_lines, line, file_extension, nesting_state, error) - CheckLanguage(filename, clean_lines, line, file_extension, include_state, - nesting_state, error) - CheckForNonConstReference(filename, clean_lines, line, nesting_state, error) - CheckForNonStandardConstructs(filename, clean_lines, line, - nesting_state, error) - CheckVlogArguments(filename, clean_lines, line, error) - CheckPosixThreading(filename, clean_lines, line, error) - CheckInvalidIncrement(filename, clean_lines, line, error) - CheckMakePairUsesDeduction(filename, clean_lines, line, error) - CheckRedundantVirtual(filename, clean_lines, line, error) - CheckRedundantOverrideOrFinal(filename, clean_lines, line, error) - for check_fn in extra_check_functions: - check_fn(filename, clean_lines, line, error) - -def FlagCxx11Features(filename, clean_lines, linenum, error): - """Flag those c++11 features that we only allow in certain places. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - include = Match(r'\s*#\s*include\s+[<"]([^<"]+)[">]', line) - - # Flag unapproved C++ TR1 headers. - if include and include.group(1).startswith('tr1/'): - error(filename, linenum, 'build/c++tr1', 5, - ('C++ TR1 headers such as <%s> are unapproved.') % include.group(1)) - - # Flag unapproved C++11 headers. - if include and include.group(1) in ('cfenv', - 'fenv.h', - 'system_error', - ): - error(filename, linenum, 'build/c++11', 5, - ('<%s> is an unapproved C++11 header.') % include.group(1)) - - # The only place where we need to worry about C++11 keywords and library - # features in preprocessor directives is in macro definitions. - if Match(r'\s*#', line) and not Match(r'\s*#\s*define\b', line): return - - # These are classes and free functions. The classes are always - # mentioned as std::*, but we only catch the free functions if - # they're not found by ADL. They're alphabetical by header. - for top_name in ( - # type_traits - 'alignment_of', - 'aligned_union', - ): - if Search(r'\bstd::%s\b' % top_name, line): - error(filename, linenum, 'build/c++11', 5, - ('std::%s is an unapproved C++11 class or function. Send c-style ' - 'an example of where it would make your code more readable, and ' - 'they may let you use it.') % top_name) - - -def FlagCxx14Features(filename, clean_lines, linenum, error): - """Flag those C++14 features that we restrict. - - Args: - filename: The name of the current file. - clean_lines: A CleansedLines instance containing the file. - linenum: The number of the line to check. - error: The function to call with any errors found. - """ - line = clean_lines.elided[linenum] - - include = Match(r'\s*#\s*include\s+[<"]([^<"]+)[">]', line) - - # Flag unapproved C++14 headers. - if include and include.group(1) in ('scoped_allocator', 'shared_mutex'): - error(filename, linenum, 'build/c++14', 5, - ('<%s> is an unapproved C++14 header.') % include.group(1)) - - -def ProcessFileData(filename, file_extension, lines, error, - extra_check_functions=[]): - """Performs lint checks and reports any errors to the given error function. - - Args: - filename: Filename of the file that is being processed. - file_extension: The extension (dot not included) of the file. - lines: An array of strings, each representing a line of the file, with the - last element being empty if the file is terminated with a newline. - error: A callable to which errors are reported, which takes 4 arguments: - filename, line number, error level, and message - extra_check_functions: An array of additional check functions that will be - run on each source line. Each function takes 4 - arguments: filename, clean_lines, line, error - """ - lines = (['// marker so line numbers and indices both start at 1'] + lines + - ['// marker so line numbers end in a known way']) - - include_state = _IncludeState() - function_state = _FunctionState() - nesting_state = NestingState() - - ResetNolintSuppressions() - - CheckForCopyright(filename, lines, error) - ProcessGlobalSuppresions(lines) - RemoveMultiLineComments(filename, lines, error) - clean_lines = CleansedLines(lines) - - if IsHeaderExtension(file_extension): - CheckForHeaderGuard(filename, clean_lines, error) - - for line in xrange(clean_lines.NumLines()): - ProcessLine(filename, file_extension, clean_lines, line, - include_state, function_state, nesting_state, error, - extra_check_functions) - FlagCxx11Features(filename, clean_lines, line, error) - nesting_state.CheckCompletedBlocks(filename, error) - - CheckForIncludeWhatYouUse(filename, clean_lines, include_state, error) - - # Check that the .cc file has included its header if it exists. - if _IsSourceExtension(file_extension): - CheckHeaderFileIncluded(filename, include_state, error) - - # We check here rather than inside ProcessLine so that we see raw - # lines rather than "cleaned" lines. - CheckForBadCharacters(filename, lines, error) - - CheckForNewlineAtEOF(filename, lines, error) - -def ProcessConfigOverrides(filename): - """ Loads the configuration files and processes the config overrides. - - Args: - filename: The name of the file being processed by the linter. - - Returns: - False if the current |filename| should not be processed further. - """ - - abs_filename = os.path.abspath(filename) - cfg_filters = [] - keep_looking = True - while keep_looking: - abs_path, base_name = os.path.split(abs_filename) - if not base_name: - break # Reached the root directory. - - cfg_file = os.path.join(abs_path, "CPPLINT.cfg") - abs_filename = abs_path - if not os.path.isfile(cfg_file): - continue - - try: - with open(cfg_file) as file_handle: - for line in file_handle: - line, _, _ = line.partition('#') # Remove comments. - if not line.strip(): - continue - - name, _, val = line.partition('=') - name = name.strip() - val = val.strip() - if name == 'set noparent': - keep_looking = False - elif name == 'filter': - cfg_filters.append(val) - elif name == 'exclude_files': - # When matching exclude_files pattern, use the base_name of - # the current file name or the directory name we are processing. - # For example, if we are checking for lint errors in /foo/bar/baz.cc - # and we found the .cfg file at /foo/CPPLINT.cfg, then the config - # file's "exclude_files" filter is meant to be checked against "bar" - # and not "baz" nor "bar/baz.cc". - if base_name: - pattern = re.compile(val) - if pattern.match(base_name): - if _cpplint_state.quiet: - # Suppress "Ignoring file" warning when using --quiet. - return False - sys.stderr.write('Ignoring "%s": file excluded by "%s". ' - 'File path component "%s" matches ' - 'pattern "%s"\n' % - (filename, cfg_file, base_name, val)) - return False - elif name == 'linelength': - global _line_length - try: - _line_length = int(val) - except ValueError: - sys.stderr.write('Line length must be numeric.') - elif name == 'root': - global _root - # root directories are specified relative to CPPLINT.cfg dir. - _root = os.path.join(os.path.dirname(cfg_file), val) - elif name == 'headers': - ProcessHppHeadersOption(val) - else: - sys.stderr.write( - 'Invalid configuration option (%s) in file %s\n' % - (name, cfg_file)) - - except IOError: - sys.stderr.write( - "Skipping config file '%s': Can't open for reading\n" % cfg_file) - keep_looking = False - - # Apply all the accumulated filters in reverse order (top-level directory - # config options having the least priority). - for filter in reversed(cfg_filters): - _AddFilters(filter) - - return True - - -def ProcessFile(filename, vlevel, extra_check_functions=[]): - """Does google-lint on a single file. - - Args: - filename: The name of the file to parse. - - vlevel: The level of errors to report. Every error of confidence - >= verbose_level will be reported. 0 is a good default. - - extra_check_functions: An array of additional check functions that will be - run on each source line. Each function takes 4 - arguments: filename, clean_lines, line, error - """ - - _SetVerboseLevel(vlevel) - _BackupFilters() - old_errors = _cpplint_state.error_count - - if not ProcessConfigOverrides(filename): - _RestoreFilters() - return - - lf_lines = [] - crlf_lines = [] - try: - # Support the UNIX convention of using "-" for stdin. Note that - # we are not opening the file with universal newline support - # (which codecs doesn't support anyway), so the resulting lines do - # contain trailing '\r' characters if we are reading a file that - # has CRLF endings. - # If after the split a trailing '\r' is present, it is removed - # below. - if filename == '-': - lines = codecs.StreamReaderWriter(sys.stdin, - codecs.getreader('utf8'), - codecs.getwriter('utf8'), - 'replace').read().split('\n') - else: - lines = codecs.open(filename, 'r', 'utf8', 'replace').read().split('\n') - - # Remove trailing '\r'. - # The -1 accounts for the extra trailing blank line we get from split() - for linenum in range(len(lines) - 1): - if lines[linenum].endswith('\r'): - lines[linenum] = lines[linenum].rstrip('\r') - crlf_lines.append(linenum + 1) - else: - lf_lines.append(linenum + 1) - - except IOError: - sys.stderr.write( - "Skipping input '%s': Can't open for reading\n" % filename) - _RestoreFilters() - return - - # Note, if no dot is found, this will give the entire filename as the ext. - file_extension = filename[filename.rfind('.') + 1:] - - # When reading from stdin, the extension is unknown, so no cpplint tests - # should rely on the extension. - if filename != '-' and file_extension not in _valid_extensions: - sys.stderr.write('Ignoring %s; not a valid file name ' - '(%s)\n' % (filename, ', '.join(_valid_extensions))) - else: - ProcessFileData(filename, file_extension, lines, Error, - extra_check_functions) - - # If end-of-line sequences are a mix of LF and CR-LF, issue - # warnings on the lines with CR. - # - # Don't issue any warnings if all lines are uniformly LF or CR-LF, - # since critique can handle these just fine, and the style guide - # doesn't dictate a particular end of line sequence. - # - # We can't depend on os.linesep to determine what the desired - # end-of-line sequence should be, since that will return the - # server-side end-of-line sequence. - if lf_lines and crlf_lines: - # Warn on every line with CR. An alternative approach might be to - # check whether the file is mostly CRLF or just LF, and warn on the - # minority, we bias toward LF here since most tools prefer LF. - for linenum in crlf_lines: - Error(filename, linenum, 'whitespace/newline', 1, - 'Unexpected \\r (^M) found; better to use only \\n') - - # Suppress printing anything if --quiet was passed unless the error - # count has increased after processing this file. - if not _cpplint_state.quiet or old_errors != _cpplint_state.error_count: - sys.stdout.write('Done processing %s\n' % filename) - _RestoreFilters() - - -def PrintUsage(message): - """Prints a brief usage string and exits, optionally with an error message. - - Args: - message: The optional error message. - """ - sys.stderr.write(_USAGE) - if message: - sys.exit('\nFATAL ERROR: ' + message) - else: - sys.exit(1) - - -def PrintCategories(): - """Prints a list of all the error-categories used by error messages. - - These are the categories used to filter messages via --filter. - """ - sys.stderr.write(''.join(' %s\n' % cat for cat in _ERROR_CATEGORIES)) - sys.exit(0) - - -def ParseArguments(args): - """Parses the command line arguments. - - This may set the output format and verbosity level as side-effects. - - Args: - args: The command line arguments: - - Returns: - The list of filenames to lint. - """ - try: - (opts, filenames) = getopt.getopt(args, '', ['help', 'output=', 'verbose=', - 'counting=', - 'filter=', - 'root=', - 'linelength=', - 'extensions=', - 'headers=', - 'quiet']) - except getopt.GetoptError: - PrintUsage('Invalid arguments.') - - verbosity = _VerboseLevel() - output_format = _OutputFormat() - filters = '' - quiet = _Quiet() - counting_style = '' - - for (opt, val) in opts: - if opt == '--help': - PrintUsage(None) - elif opt == '--output': - if val not in ('emacs', 'vs7', 'eclipse'): - PrintUsage('The only allowed output formats are emacs, vs7 and eclipse.') - output_format = val - elif opt == '--quiet': - quiet = True - elif opt == '--verbose': - verbosity = int(val) - elif opt == '--filter': - filters = val - if not filters: - PrintCategories() - elif opt == '--counting': - if val not in ('total', 'toplevel', 'detailed'): - PrintUsage('Valid counting options are total, toplevel, and detailed') - counting_style = val - elif opt == '--root': - global _root - _root = val - elif opt == '--linelength': - global _line_length - try: - _line_length = int(val) - except ValueError: - PrintUsage('Line length must be digits.') - elif opt == '--extensions': - global _valid_extensions - try: - _valid_extensions = set(val.split(',')) - except ValueError: - PrintUsage('Extensions must be comma seperated list.') - elif opt == '--headers': - ProcessHppHeadersOption(val) - - if not filenames: - PrintUsage('No files were specified.') - - _SetOutputFormat(output_format) - _SetQuiet(quiet) - _SetVerboseLevel(verbosity) - _SetFilters(filters) - _SetCountingStyle(counting_style) - - return filenames - - -def main(): - filenames = ParseArguments(sys.argv[1:]) - - # Change stderr to write with replacement characters so we don't die - # if we try to print something containing non-ASCII characters. - sys.stderr = codecs.StreamReaderWriter(sys.stderr, - codecs.getreader('utf8'), - codecs.getwriter('utf8'), - 'replace') - - _cpplint_state.ResetErrorCounts() - for filename in filenames: - ProcessFile(filename, _cpplint_state.verbose_level) - # If --quiet is passed, suppress printing error count unless there are errors. - if not _cpplint_state.quiet or _cpplint_state.error_count > 0: - _cpplint_state.PrintErrorCounts() - - sys.exit(_cpplint_state.error_count > 0) - - -if __name__ == '__main__': - main() diff --git a/tools/cpplint_to_cppcheckxml.py b/tools/cpplint_to_cppcheckxml.py deleted file mode 100644 index 93ba7a25c5..0000000000 --- a/tools/cpplint_to_cppcheckxml.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python - -# Convert output from Google's cpplint.py to the cppcheck XML format for -# consumption by the Jenkins cppcheck plugin. - -# Reads from stdin and writes to stderr (to mimic cppcheck) - - -import sys -import re -import xml.sax.saxutils - -def cpplint_score_to_cppcheck_severity(score): - # I'm making this up - if score == 1: - return 'style' - elif score == 2: - return 'style' - elif score == 3: - return 'warning' - elif score == 4: - return 'warning' - elif score == 5: - return 'error' - - -def parse(): - # TODO: do this properly, using the xml module. - # Write header - sys.stderr.write('''\n''') - sys.stderr.write('''\n''') - - # Do line-by-line conversion - r = re.compile('([^:]*):([0-9]*): ([^\[]*)\[([^\]]*)\] \[([0-9]*)\].*') - - for l in sys.stdin.readlines(): - m = r.match(l.strip()) - if not m: - continue - g = m.groups() - if len(g) != 5: - continue - fname, lineno, rawmsg, label, score = g - # Protect Jenkins from bad XML, which makes it barf - msg = xml.sax.saxutils.quoteattr(rawmsg) - severity = cpplint_score_to_cppcheck_severity(int(score)) - sys.stderr.write('''\n'''%(fname, lineno, label, severity, msg)) - - # Write footer - sys.stderr.write('''\n''') - - -if __name__ == '__main__': - parse() diff --git a/tutorials.md.in b/tutorials.md.in index 934f01e3b3..ab7c1c8445 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -27,9 +27,9 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation. * \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. -* \subpage collada_world_exporter "Collada World Exporter": Export an entire -world to a single Collada mesh. +* \subpage collada_world_exporter "Collada World Exporter": Export an entire world to a single Collada mesh. * \subpage underwater_vehicles "Underwater Vehicles": Understand how to simulate underwater vehicles. +* \subpage particle_mitter "Particle emitter": Using particle emitters in simulation **Migration from Gazebo classic** diff --git a/tutorials/files/particle_emitter/fog_generator.png b/tutorials/files/particle_emitter/fog_generator.png new file mode 100644 index 0000000000..a97f51413e Binary files /dev/null and b/tutorials/files/particle_emitter/fog_generator.png differ diff --git a/tutorials/files/particle_emitter/particle_scatter_ratio.png b/tutorials/files/particle_emitter/particle_scatter_ratio.png new file mode 100644 index 0000000000..57535bb407 Binary files /dev/null and b/tutorials/files/particle_emitter/particle_scatter_ratio.png differ diff --git a/tutorials/files/particle_emitter/sensor_scatter_tutorial.gif b/tutorials/files/particle_emitter/sensor_scatter_tutorial.gif new file mode 100644 index 0000000000..4663b2ac5b Binary files /dev/null and b/tutorials/files/particle_emitter/sensor_scatter_tutorial.gif differ diff --git a/tutorials/particle_tutorial.md b/tutorials/particle_tutorial.md new file mode 100644 index 0000000000..e996b47039 --- /dev/null +++ b/tutorials/particle_tutorial.md @@ -0,0 +1,110 @@ +\page particle_mitter Particle Emitter + +This tutorial shows how to use the particle emitter system to add and configure particle effects like smoke and fog in simulation. It also shows the effects that particles have on different types of sensors in Ignition Gazebo. + +## Particle Emitter System + +We will demonstrate the particle emitter system by using the [examples/worlds/particle_emitter2.sdf]( +https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/examples/worlds/particle_emitter2.sdf) world. + +To be able to spawn particle emitters, first you will need to include the particle emitter system as a plugin to the world in your SDF. The system does not take any arguments. + +```xml + + +``` + +Next, we can start adding particle emitter models into the world. In our example world, we include a [Fog Generator](https://app.ignitionrobotics.org/OpenRobotics/fuel/models/Fog%20Generator) model from Ignition Fuel: + +```xml + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/fog generator + +``` + +Here is the content of the Fog Generator [model.sdf](https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fog%20Generator/1/files/model.sdf) file. + +```xml + + 0 0 0 0 -1.5707 0 + true + + + true + 10 10 0 + 1 1 1 + 25 + 0.1 + 0.2 + 0.5 + 5 + + 0.7 0.7 0.7 + 1.0 1.0 1.0 + + + materials/textures/fog.png + + + + materials/textures/fogcolors.png + + + +``` + +The SDF 1.6+ specification supports having a `` SDF element as a child of ``. The particle emitter itself has several properties that can be configured, see Ignition Rendering's [particles tutorial](https://ignitionrobotics.org/api/rendering/4.0/particles.html) for more details on these properties. In our Fog Generator model, we are using a box type particle emitter that covers a region size of 10 by 10m. By default, the particles are emitted in the `+x` direction, hence the model as a pitch rotation of -90 degrees to rotate the particle emitter so that the particles are emitted upwards in `+z`. + +Let's launch the example world to see what it looks like. + +```bash +ign gazebo -v 4 -r particle_emitter2.sdf +``` + +You should see the fog slowly starting to appear from the ground plane in the world: + +@image html files/particle_emitter/fog_generator.png + +Next, try changing some properties of the particle emitter while the simulation is running. You can do this by publishing messages over Ignition Transport. Try turning off the particle emitter by setting the `emitting` property to `false`. Make sure the simulation is running in order for this command to take effect. + +```bash +ign topic -t /model/fog_generator/link/fog_link/particle_emitter/emitter/cmd -m ignition.msgs.ParticleEmitter -p 'emitting: {data: false}' +``` + +Note the above command tells the particle emitter to stop emitting. It does not make all the particles disappear immediately. The particles that have already been emitted will naturally fade and disappear over the specified `lifetime`. + +Turn particle emitter back on by setting the `emitting` property to `true`: + +```bash +ign topic -t /model/fog_generator/link/fog_link/particle_emitter/emitter/cmd -m ignition.msgs.ParticleEmitter -p 'emitting: {data: true}' +``` + +Here is an example command for changing the `rate` property of the particle emitter. This should make the particle emitter emit more particles per second, causing it to visually appear more dense. + +```bash +ign topic -t /model/fog_generator/link/fog_link/particle_emitter/emitter/cmd -m ignition.msgs.ParticleEmitter -p 'rate: {data: 100}' +``` + +## Particle Effects on Sensors + +The particles are not only a visual effect in simulation, they also have an effect on sensors in simulation. Here are the sensor types and the effects the particle emitter have on them: + +* `camera`: The particles are visible to a regular camera sensor +* `depth_camera`: The particles have a scattering effect on the depth data. +* `rgbd_camera`: The particles are visible in the RGB image and have a scattering effect on the depth data. +* `gpu_lidar`: The particles have a scattering effect on the lidar range readings. +* `thermal_camera`: The particles are not visible in the thermal camera image. + +The gif below shows an [example world](https://gist.github.com/iche033/bcd3b7d3f4874e1e707e392d6dbb0aa0) with six different sensors looking at the fog generator with a rescue randy model inside the fog. + +@image html files/particle_emitter/sensor_scatter_tutorial.gif + +The two image displays on the left show the images from a regular camera sensor and RGB output of a RGBD camera. The two have very similar color image output that shows the fog in the camera view. To their right are the depth images produced by a depth camera and the depth output of the RGBD camera. The depth readings are noisy due to the particle scattering effect. You can also see that these sensors can partially see through the fog and detect the rescue randy inside it. The bottom left of the gif shows the lidar visualization; the range data are also affected by the scattering effect. Finally, the thermal camera image display on the bottom right shows that thermal cameras do not detect particles. + +The sensor scattering effect can be configured by adding `` to the `` SDF element. This property determines the ratio of particles that will be detected by sensors. Increasing the ratio means there is a higher chance of particles reflecting and interfering with depth sensing, making the emitter appear more dense. Decreasing the ratio decreases the chance of particles reflecting and interfering with depth sensing, making it appear less dense. + +See image below that reduces the particle scatter ratio to 0.1. The depth camera image, RGBD camera's depth image, and lidar's range data are noticeably less noisy than the gif above. + +@image html files/particle_emitter/particle_scatter_ratio.png diff --git a/tutorials/physics.md b/tutorials/physics.md index c86ecb07a7..4ff2a32097 100644 --- a/tutorials/physics.md +++ b/tutorials/physics.md @@ -84,6 +84,18 @@ serverConfig.SetPhysicsEngine("CustomEngine"); ignition::gazebo::Server server(serverConfig); ``` +## Engine configuration + +Gazebo supports the following physics engine configurations through SDF. +These options are available to all physics engines, but not all engines +may support them. The default physics engine, DART, supports all these options. + +* [//physics/dart/collision_detector](http://sdformat.org/spec?ver=1.8&elem=physics#dart_collision_detector) + * Options supported by DART: `ode` (default), `bullet`, `fcl`, `dart`. + +* [//physics/dart/solver/solver_type](http://sdformat.org/spec?ver=1.8&elem=physics#solver_solver_type) + * Options supported by DART: `dantzig` (default), `pgs` + ## Troubleshooting > Failed to find plugin [libCustomEngine.so]. Have you checked the