diff --git a/CMakeLists.txt b/CMakeLists.txt index 45d4ac54..5038e465 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,6 @@ if (DETECTED_ROS2) find_package(rclcpp REQUIRED) - # *** TEMPORARY ***!!! find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -193,15 +192,10 @@ endif() # Dependency: MRPT # Use the first cmd to set the minimum required version # ---------------------------------------------------------- -find_package(mrpt-maps REQUIRED) -find_package(mrpt-tclap REQUIRED) -find_package(mrpt-gui REQUIRED) -find_package(mrpt-tfest REQUIRED) -find_package(mrpt-topography REQUIRED) - -if (mrpt-maps_VERSION VERSION_LESS "2.14.1") - message(FATAL_ERROR "MRPT version 2.14.1 or newer is required, but found ${mrpt-maps_VERSION}") -endif() +find_package(mrpt_maps REQUIRED) +find_package(mrpt_gui REQUIRED) +find_package(mrpt_tfest REQUIRED) +find_package(mrpt_topography REQUIRED) # -------------------------- # Dependency: Box2D diff --git a/definitions/jackal.vehicle.xml b/definitions/jackal.vehicle.xml index ea1311c1..e4579a25 100644 --- a/definitions/jackal.vehicle.xml +++ b/definitions/jackal.vehicle.xml @@ -30,8 +30,8 @@ - 20 - 10 + 18.2909 + 474.4889 0.0000 100 diff --git a/docs/mvsim-pid-tuner.rst b/docs/mvsim-pid-tuner.rst index d5340015..e80ab33f 100644 --- a/docs/mvsim-pid-tuner.rst +++ b/docs/mvsim-pid-tuner.rst @@ -4,8 +4,9 @@ mvsim-pid-tuner =============== ``mvsim-pid-tuner`` is a command-line tool for automatic PID parameter tuning of MVSim -vehicle controllers. It identifies the vehicle's dynamic response and proposes optimal -PID gains using the IMC (Internal Model Control) tuning method. +vehicle controllers. It identifies the vehicle's dynamic response (both linear and +rotational) and proposes optimal PID gains using the IMC (Internal Model Control) +tuning method. .. contents:: :depth: 1 @@ -18,27 +19,38 @@ Overview Tuning PID controllers for simulated vehicles can be tedious. This tool automates the process by: -1. Running an **open-loop step response** test (constant torque applied to wheels) -2. **Identifying the plant model** as a first-order system: :math:`G(s) = K / (\tau s + 1)` -3. Computing **optimal PID parameters** using IMC tuning rules -4. **Validating** the proposed parameters with a closed-loop step-up/step-down simulation +1. Running **open-loop step response** tests for both linear and rotational motion +2. **Identifying the plant models** as first-order systems: :math:`G(s) = K / (\tau s + 1)` +3. Computing **optimal PID parameters** using IMC tuning rules, balanced for both modes +4. **Validating** the proposed parameters with closed-loop step-up/step-down simulations + for both linear velocity and angular velocity Algorithm --------- -**Phase 1: Plant Identification** +**Phase 1a: Linear Plant Identification** -A constant torque is applied to all wheels (via a ``raw`` controller) and the average -wheel velocity is recorded over time. The steady-state velocity :math:`v_{ss}` is -computed from the last 20% of samples, and the plant gain and time constant are: +A constant torque is applied equally to all wheels (via a ``raw`` controller) and the +average wheel velocity is recorded over time. The steady-state velocity :math:`v_{ss}` +is computed from the last 20% of samples, and the plant gain and time constant are: .. math:: - K = \frac{v_{ss}}{\tau_{applied}}, \quad \tau = \text{time to reach } 63.2\% \text{ of } v_{ss} + K_{lin} = \frac{v_{ss}}{\tau_{applied}}, \quad \tau_{lin} = \text{time to reach } 63.2\% \text{ of } v_{ss} + +**Phase 1b: Rotation Plant Identification** + +Opposite torques are applied to left and right wheels (left forward, right backward) +to induce pure rotation. The angular velocity :math:`\omega` is recorded from the +vehicle's odometry estimate. The rotational plant model is identified analogously: + +.. math:: + + K_{rot} = \frac{\omega_{ss}}{\tau_{applied}}, \quad \tau_{rot} = \text{time to reach } 63.2\% \text{ of } \omega_{ss} **Phase 2: IMC PID Tuning** -For a first-order plant, the IMC method yields a PI controller: +For each first-order plant (linear and rotational), the IMC method yields a PI controller: .. math:: @@ -47,12 +59,20 @@ For a first-order plant, the IMC method yields a PI controller: where :math:`\tau_{cl} = \tau \cdot \alpha` is the desired closed-loop time constant and :math:`\alpha` is the aggressiveness factor (smaller = faster response). +Since the ``twist_pid`` controller uses the same PID gains for both wheels, the tool +computes PID parameters for both linear and rotational plants independently, then uses +the **geometric mean** to balance performance across both modes. + The ``max_torque`` is set to 80% of the estimated friction limit to avoid wheel slip. **Phase 3: Closed-Loop Validation** -The proposed PID parameters are tested with a step-up to 1.0 m/s (3 seconds) followed -by a step-down to 0 m/s (3 seconds). The tool reports: +The proposed PID parameters are validated with two separate tests: + +* **Linear test**: step-up to 1.0 m/s (3 seconds) followed by step-down to 0 m/s (3 seconds) +* **Rotation test**: step-up to 1.0 rad/s (3 seconds) followed by step-down to 0 rad/s (3 seconds) + +For each test, the tool reports: * Rise time (time to 90% of setpoint) * Settling time (time to enter 2% band) @@ -111,41 +131,65 @@ Sample output: MVSim PID Auto-Tuner ====================================== - --- Phase 1: Open-loop plant identification --- + --- Phase 1a: Open-loop LINEAR plant identification --- Vehicle: small_robot (2 wheels) Wheel 0: pos=(0.000, 0.500) R=0.200 mass=4.00 Wheel 1: pos=(0.000, -0.500) R=0.200 mass=4.00 Chassis mass: 15.0 kg + Wheel track: 1.000 m Estimated max friction torque per wheel: 18.10 Nm (mu=0.8) - Test torque: 9.05 Nm + Test torque: 9.05 Nm (linear) - === Open-Loop Step Response Analysis === + === Open-Loop Step Response Analysis (linear) === Applied torque: 9.050 Nm Steady-state velocity: 1.2345 m/s Plant gain K: 0.1364 (m/s)/Nm - Time constant τ: 0.1500 s + Time constant tau: 0.1500 s + + --- Phase 1b: Open-loop ROTATION plant identification --- + Test torque: 9.05 Nm (rotation) + + === Open-Loop Step Response Analysis (rotation) === + Applied torque: 9.050 Nm + Steady-state omega: 2.5000 rad/s + Plant gain K: 0.2762 (rad/s)/Nm + Time constant tau: 0.0800 s --- Phase 2: IMC PID tuning (aggressiveness=0.25) --- - Proposed PID parameters: - KP: 29.3200 - KI: 195.4667 + Linear-only PID: KP=29.3200 KI=195.4667 + Rotation-only PID: KP=14.4800 KI=181.0000 + + Proposed PID parameters (balanced linear + rotation): + KP: 20.6000 + KI: 188.0000 KD: 0.0000 max_torque: 14.48 Nm --- Phase 3: Closed-loop validation --- + + [Linear] Step-up (1.0 m/s, 3s) then step-down (stop, 3s)... Step-up (0 -> 1.0 m/s): + Rise time (90%): 0.150 s + Settling time (2%): 0.300 s + Overshoot: 3.2 % + Steady-state error: 0.0015 m/s + Step-down (1.0 -> 0 m/s): + Settling time (2%): 0.350 s + Rebound: none + + [Rotation] Step-up (1.0 rad/s, 3s) then step-down (stop, 3s)... + Step-up (0 -> 1.0 rad/s): Rise time (90%): 0.120 s Settling time (2%): 0.250 s - Overshoot: 2.5 % - Steady-state error: 0.0012 m/s - - Step-down (1.0 -> 0 m/s): + Overshoot: 2.8 % + Steady-state error: 0.0020 rad/s + Step-down (1.0 -> 0 rad/s): Settling time (2%): 0.300 s Rebound: none --- Assessment --- - All metrics look good! + All metrics look good for both linear and rotation! The tool prints an XML snippet with the proposed PID values that can be pasted directly into the vehicle's ```` block. diff --git a/docs/world_lighting.rst b/docs/world_lighting.rst index 173cd31e..af224356 100644 --- a/docs/world_lighting.rst +++ b/docs/world_lighting.rst @@ -4,23 +4,28 @@ Light and shadows configuration -------------------------------------------- -At present, the lighting model of ``mrpt-opengl`` defines -**one directional light source** (i.e. placed at the infinity), -with one color and three component intensities (ambient, diffuse, and specular). -See `mrpt::opengl::TLightParameters `_ +The lighting model uses MRPT's multi-light rendering pipeline with +**hemisphere ambient lighting**, a **directional light source** (placed at infinity) +that casts shadows, and optional **point** and **spot** light sources. +See `mrpt::viz::TLightParameters `_ and `mrpt-opengl `_ for further details. -MVSim offers a number of parameters under the global `` ... `` tag +MVSim offers a number of parameters under the global `` ... `` tag to tune the performance and appearance of lights and shadows (all are optional). -Light control -================ +Directional light +=================== + +The primary directional light (sun-like, infinitely far away, parallel rays) is always present. +Its direction is controlled via azimuth and elevation angles, and it is the only light that casts shadows. - ``#ffffff``: The light color (see formatting for :ref:`%color `). -- ``0.5``: Ambient component of the light (0 to 1). +- ``0.8``: Diffuse intensity of the directional light (0 to 1). -- ``45.0`` and ``70.0``: +- ``0.6``: Specular intensity of the directional light (0 to 1). + +- ``45.0`` and ``70.0``: The orbit-like azimuth and elevation angles (in **degrees**) of the directional light source. For example, an elevation of ``90`` means a pure vertical (downwards) light. @@ -29,12 +34,76 @@ Light control +Ambient lighting +================= + +MVSim uses **hemisphere ambient lighting**: surfaces facing up receive the sky ambient color, +surfaces facing down receive the ground ambient color, with smooth interpolation for +surfaces at intermediate angles. This produces a much more natural look than flat ambient, +especially for outdoor scenes. + +- ``0.4``: Overall ambient intensity scale (0 to 1). + +- ``#e0e8ff``: The ambient color for upward-facing surfaces. + Default is a slight blue tint (``#e0e8ff``), simulating sky light. + +- ``#403a30``: The ambient color for downward-facing surfaces. + Default is a dark warm brown (``#403a30``), simulating ground-bounce light. + +To get flat (non-hemisphere) ambient lighting, set both colors to the same value (e.g. ``#ffffff``). + + +Point lights +============= + +Point lights emit in all directions from a position in world coordinates. +They do **not** cast shadows. Multiple point lights can be defined by adding +```` child elements inside ````. + +Each ```` supports the following child tags (all optional, shown with defaults): + +- ``X Y Z``: Position in world coordinates (default: ``0 0 3``). +- ``#ffffff``: Light color in ``#RRGGBB[AA]`` format (default: white). +- ``0.8``: Diffuse intensity (0 to 1). +- ``0.5``: Specular intensity (0 to 1). +- ``1.0``: Constant attenuation factor. +- ``0.09``: Linear attenuation factor. +- ``0.032``: Quadratic attenuation factor. + +The light intensity at distance *d* is: ``1 / (constant + linear*d + quadratic*d²)``. + + +Spot lights +============ + +Spot lights emit in a cone from a position along a direction. +They do **not** cast shadows. Multiple spot lights can be defined by adding +```` child elements inside ````. + +Each ```` supports the following child tags (all optional, shown with defaults): + +- ``X Y Z``: Position in world coordinates (default: ``0 0 3``). +- ``X Y Z``: Direction vector the spot points towards (default: ``0 0 -1``). +- ``#ffffff``: Light color in ``#RRGGBB[AA]`` format (default: white). +- ``0.8``: Diffuse intensity (0 to 1). +- ``0.5``: Specular intensity (0 to 1). +- ``12.5``: Inner cone half-angle in degrees (full intensity). +- ``17.5``: Outer cone half-angle in degrees (light fades to zero). +- ``1.0``: Constant attenuation factor. +- ``0.09``: Linear attenuation factor. +- ``0.032``: Quadratic attenuation factor. + +.. note:: + + Up to 8 simultaneous light sources are supported (including the primary directional light). + This limit is defined by the shader pipeline. + Shadows control ================ - ``true``. Can be used to disable casting shadows (enabled by default). - Note that shadows may not be visible on certain ground objects, most notably, occupancy grid maps, so + Note that shadows may not be visible on certain ground objects, most notably, occupancy grid maps, so shadows not being visible on grids is not a bug. .. raw:: html @@ -47,7 +116,7 @@ Shadows control - ``2048``. The resolution of the `shadow map texture `_ in pixels. Larger values will give more well-defined shadow borders, at the cost of higher GPU usage. Smaller values may slightly improve speed. -.. list-table:: +.. list-table:: * - .. figure:: https://mrpt.github.io/mvsim-models/screenshots/shadow_map_size-512.png @@ -67,8 +136,8 @@ Shadows control shadows, defined by ``light_clip_plane_min``. Ideally, the smaller the range between these two numbers, the more accurate shadows will be. -- ``1e-5``, ``1e-5``, and - ``1e-4`` are all ``mrpt-opengl`` heuristic parameters to +- ``1e-5``, ``1e-5``, and + ``1e-4`` are all ``mrpt-opengl`` heuristic parameters to fight the "Peter-Panning" and "shadow acne" artifacts in shadow casting [Microsoft_Shadows]_. - ``2.0``: This unitless (meter/meter) @@ -83,18 +152,46 @@ Shadows control .. code-block:: xml - :caption: Light and shadow tuning parameters: example + :caption: Complete lighting configuration example ... - - - true - - + + 160.0 40.0 - + #ffffff + 0.8 + 0.6 + + + 0.4 + #e0e8ff + #403a30 + + + true + + + + + 5.0 3.0 4.0 + #ffe0a0 + 0.6 + 0.3 + 0.05 + + + + + 0.0 0.0 5.0 + 0.0 0.0 -1.0 + #ffffff + 0.7 + 15.0 + 25.0 + + ... diff --git a/examples_cpp/mvsim-subscriber-example/CMakeLists.txt b/examples_cpp/mvsim-subscriber-example/CMakeLists.txt index 55dd0a20..18b6462e 100644 --- a/examples_cpp/mvsim-subscriber-example/CMakeLists.txt +++ b/examples_cpp/mvsim-subscriber-example/CMakeLists.txt @@ -3,12 +3,12 @@ project(mvsim-subscriber-example) find_package(mvsim-comms) find_package(mvsim-msgs) -find_package(mrpt-obs) +find_package(mrpt_obs) add_executable(${PROJECT_NAME} ${PROJECT_NAME}.cpp) target_link_libraries(${PROJECT_NAME} mvsim::mvsim-comms mvsim::mvsim-msgs - mrpt::obs + mrpt::mrpt_obs ) diff --git a/modules/comms/CMakeLists.txt b/modules/comms/CMakeLists.txt index 2eeb043f..4cd28d72 100644 --- a/modules/comms/CMakeLists.txt +++ b/modules/comms/CMakeLists.txt @@ -28,9 +28,9 @@ target_link_libraries(${PROJECT_NAME} ${ZeroMQ_LIBRARY} PUBLIC ${Protobuf_LIBRARIES} - mrpt::system - mrpt::io - mrpt::serialization + mrpt::mrpt_system + mrpt::mrpt_io + mrpt::mrpt_serialization mvsim::msgs ) diff --git a/modules/simulator/CMakeLists.txt b/modules/simulator/CMakeLists.txt index 9c5a69be..048e8b0d 100644 --- a/modules/simulator/CMakeLists.txt +++ b/modules/simulator/CMakeLists.txt @@ -150,11 +150,11 @@ target_link_libraries(${PROJECT_NAME} PRIVATE ${DEP_MVSIM_MSGS_TRG} PUBLIC - mrpt::obs - mrpt::opengl - mrpt::maps - mrpt::gui - mrpt::topography + mrpt::mrpt_obs + mrpt::mrpt_opengl + mrpt::mrpt_maps + mrpt::mrpt_gui + mrpt::mrpt_topography ${DEP_MVSIM_COMMS_TRG} ${BOX2D_LIBRARIES} ) diff --git a/modules/simulator/include/mvsim/Block.h b/modules/simulator/include/mvsim/Block.h index 737a04c7..54d06493 100644 --- a/modules/simulator/include/mvsim/Block.h +++ b/modules/simulator/include/mvsim/Block.h @@ -15,10 +15,10 @@ #include #include #include -#include -#include #include #include +#include +#include #include #include #include @@ -47,7 +47,7 @@ enum class GeometryType : int32_t /** A non-vehicle "actor" for the simulation, typically obstacle blocks. * \ingroup mvsim_simulator_module */ -class Block : public VisualObject, public Simulable +class Block : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; @@ -143,14 +143,14 @@ class Block : public VisualObject, public Simulable /// explicitly yet. Used while parsing the shape_from_visual tag. bool default_block_z_min_max() const; - VisualObject* meAsVisualObject() override { return this; } + CVisualObject* meAsCVisualObject() override { return this; } std::optional getElevationAt(const mrpt::math::TPoint2D& worldXY) const override; protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; /** user-supplied index number: must be set/get'ed with setblockIndex() * getblockIndex() (default=0) */ @@ -213,7 +213,7 @@ class Block : public VisualObject, public Simulable b2Fixture* fixture_block_; private: - void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene); + void internal_internalGuiUpdate_forces(mrpt::viz::Scene& scene); void internal_parseGeometry(const rapidxml::xml_node& xml_geom_node); @@ -242,8 +242,8 @@ class Block : public VisualObject, public Simulable GeometryParams geomParams_; - mrpt::opengl::CSetOfObjects::Ptr gl_block_; - mrpt::opengl::CSetOfLines::Ptr gl_forces_; + mrpt::viz::CSetOfObjects::Ptr gl_block_; + mrpt::viz::CSetOfLines::Ptr gl_forces_; std::mutex force_segments_for_rendering_cs_; std::vector force_segments_for_rendering_; @@ -253,7 +253,7 @@ class Block : public VisualObject, public Simulable * \ingroup mvsim_simulator_module * */ -class DummyInvisibleBlock : public VisualObject, public Simulable +class DummyInvisibleBlock : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; @@ -301,8 +301,8 @@ class DummyInvisibleBlock : public VisualObject, public Simulable protected: void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) override; void registerOnServer(mvsim::Client& c) override diff --git a/modules/simulator/include/mvsim/CollisionShapeCache.h b/modules/simulator/include/mvsim/CollisionShapeCache.h index 40a49267..4c9eaf5e 100644 --- a/modules/simulator/include/mvsim/CollisionShapeCache.h +++ b/modules/simulator/include/mvsim/CollisionShapeCache.h @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include #include @@ -24,7 +24,7 @@ class CollisionShapeCache /** Computes the convex hull of a given model, or gets it from the cache */ Shape2p5 get( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, + mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale, const std::optional& modelFile = std::nullopt); @@ -45,11 +45,11 @@ class CollisionShapeCache * filled in. */ std::optional processSimpleGeometries( - const mrpt::opengl::CRenderizable& obj, float zMin, float zMax, + const mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale); Shape2p5 processGenericGeometry( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, + mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale); Shape2p5 processCylinderLike( diff --git a/modules/simulator/include/mvsim/HumanActor.h b/modules/simulator/include/mvsim/HumanActor.h index dbecf28f..56251843 100644 --- a/modules/simulator/include/mvsim/HumanActor.h +++ b/modules/simulator/include/mvsim/HumanActor.h @@ -8,11 +8,11 @@ #pragma once -#include -#include -#include #include #include +#include +#include +#include #include #include #include @@ -21,9 +21,9 @@ #define MIN_MRPT_VERSION_ANIMATED_ASSIMP 0x020f08 #if MRPT_VERSION >= MIN_MRPT_VERSION_ANIMATED_ASSIMP -#include +#include #else -#include +#include #endif #include @@ -63,7 +63,7 @@ class World; * * \ingroup mvsim_simulator_module */ -class HumanActor : public VisualObject, public Simulable +class HumanActor : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; @@ -153,8 +153,8 @@ class HumanActor : public VisualObject, public Simulable protected: void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; // ==================== Configuration Parameters ==================== @@ -208,13 +208,14 @@ class HumanActor : public VisualObject, public Simulable std::string manualAnimationName_; double currentAnimTime_ = 0.0; + std::string currentAnimName_; //!< current anim clip name (updated by sim thread, read by GUI thread) double currentMovementSpeed_ = 0.0; //!< actual speed for anim sync // ==================== Rendering ==================== #if MRPT_VERSION >= MIN_MRPT_VERSION_ANIMATED_ASSIMP - mrpt::opengl::CAnimatedAssimpModel::Ptr glModel_; + mrpt::viz::CAnimatedAssimpModel::Ptr glModel_; #else - mrpt::opengl::CAssimpModel::Ptr glModel_; + mrpt::viz::CAssimpModel::Ptr glModel_; #endif bool glInitialized_ = false; diff --git a/modules/simulator/include/mvsim/Joystick.h b/modules/simulator/include/mvsim/Joystick.h index 9c6d171d..9f356af2 100644 --- a/modules/simulator/include/mvsim/Joystick.h +++ b/modules/simulator/include/mvsim/Joystick.h @@ -19,7 +19,7 @@ +------------------------------------------------------------------------+ */ #pragma once -#include +#include #include @@ -78,7 +78,7 @@ class Joystick /** Gets joystick information. * * This method will try first to open the joystick, so you can safely call - * it while the joystick is plugged and removed arbitrarly. + * it while the joystick is plugged and removed arbitrarily. * * \param nJoy The index of the joystick to query: The first one is 0, the * second 1, etc... See CJoystick::getJoysticksCount to discover the number diff --git a/modules/simulator/include/mvsim/Sensors/CameraSensor.h b/modules/simulator/include/mvsim/Sensors/CameraSensor.h index 26ed01ba..e22832e7 100644 --- a/modules/simulator/include/mvsim/Sensors/CameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/CameraSensor.h @@ -34,14 +34,14 @@ class CameraSensor : public SensorBase virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; - void simulateOn3DScene(mrpt::opengl::COpenGLScene& gl_scene) override; + void simulateOn3DScene(mrpt::viz::Scene& gl_scene) override; void freeOpenGLResources() override; protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -71,7 +71,7 @@ class CameraSensor : public SensorBase float rgbClipMin_ = 1e-2, rgbClipMax_ = 1e+4; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h index 8972cb60..d4d2e457 100644 --- a/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h +++ b/modules/simulator/include/mvsim/Sensors/DepthCameraSensor.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class DepthCameraSensor : public SensorBase virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; - void simulateOn3DScene(mrpt::opengl::COpenGLScene& gl_scene) override; + void simulateOn3DScene(mrpt::viz::Scene& gl_scene) override; void freeOpenGLResources() override; @@ -61,8 +61,8 @@ class DepthCameraSensor : public SensorBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -90,7 +90,7 @@ class DepthCameraSensor : public SensorBase /** Whether gl_scan_ has to be updated upon next call of * internalGuiUpdate() from last_scan2gui_ */ bool gui_uptodate_ = false; - mrpt::opengl::CPointCloudColoured::Ptr gl_obs_; + mrpt::viz::CPointCloudColoured::Ptr gl_obs_; std::optional has_to_render_; std::mutex has_to_render_mtx_; @@ -108,8 +108,8 @@ class DepthCameraSensor : public SensorBase float depth_noise_sigma_ = 1e-3; bool show_3d_pointcloud_ = false; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_fov_, gl_sensor_frustum_; mrpt::math::CMatrixFloat depthImage_; // to avoid memory allocs }; diff --git a/modules/simulator/include/mvsim/Sensors/GNSS.h b/modules/simulator/include/mvsim/Sensors/GNSS.h index 288cbd98..59cc7751 100644 --- a/modules/simulator/include/mvsim/Sensors/GNSS.h +++ b/modules/simulator/include/mvsim/Sensors/GNSS.h @@ -47,8 +47,8 @@ class GNSS : public SensorBase protected: void internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -73,7 +73,7 @@ class GNSS : public SensorBase /** Last simulated obs */ mrpt::obs::CObservationGPS::Ptr last_obs_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::random::CRandomGenerator rng_; }; diff --git a/modules/simulator/include/mvsim/Sensors/IMU.h b/modules/simulator/include/mvsim/Sensors/IMU.h index dba5f412..d5089365 100644 --- a/modules/simulator/include/mvsim/Sensors/IMU.h +++ b/modules/simulator/include/mvsim/Sensors/IMU.h @@ -43,8 +43,8 @@ class IMU : public SensorBase protected: void internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -73,7 +73,7 @@ class IMU : public SensorBase /** Last simulated obs */ mrpt::obs::CObservationIMU::Ptr last_obs_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; mrpt::random::CRandomGenerator rng_; }; diff --git a/modules/simulator/include/mvsim/Sensors/LaserScanner.h b/modules/simulator/include/mvsim/Sensors/LaserScanner.h index b06312f2..4eb0f8e4 100644 --- a/modules/simulator/include/mvsim/Sensors/LaserScanner.h +++ b/modules/simulator/include/mvsim/Sensors/LaserScanner.h @@ -11,8 +11,8 @@ #include #include -#include #include +#include #include #include @@ -41,15 +41,15 @@ class LaserScanner : public SensorBase virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; - void simulateOn3DScene(mrpt::opengl::COpenGLScene& gl_scene) override; + void simulateOn3DScene(mrpt::viz::Scene& gl_scene) override; void freeOpenGLResources() override; void registerOnServer(mvsim::Client& c) override; protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -99,9 +99,9 @@ class LaserScanner : public SensorBase * internalGuiUpdate() from last_scan2gui_ */ bool gui_uptodate_ = false; - mrpt::opengl::CPlanarLaserScan::Ptr gl_scan_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_; + mrpt::viz::CPlanarLaserScan::Ptr gl_scan_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_fov_; std::optional has_to_render_; std::mutex has_to_render_mtx_; diff --git a/modules/simulator/include/mvsim/Sensors/Lidar3D.h b/modules/simulator/include/mvsim/Sensors/Lidar3D.h index cd517e04..1360da77 100644 --- a/modules/simulator/include/mvsim/Sensors/Lidar3D.h +++ b/modules/simulator/include/mvsim/Sensors/Lidar3D.h @@ -12,8 +12,8 @@ #include #include #include -#include #include +#include #include #include @@ -39,13 +39,13 @@ class Lidar3D : public SensorBase virtual void simul_pre_timestep(const TSimulContext& context) override; virtual void simul_post_timestep(const TSimulContext& context) override; - void simulateOn3DScene(mrpt::opengl::COpenGLScene& gl_scene) override; + void simulateOn3DScene(mrpt::viz::Scene& gl_scene) override; void freeOpenGLResources() override; protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; void notifySimulableSetPose(const mrpt::math::TPose3D& newPose) override; @@ -88,9 +88,9 @@ class Lidar3D : public SensorBase * internalGuiUpdate() from last_scan2gui_ */ bool gui_uptodate_ = false; - mrpt::opengl::CPointCloudColoured::Ptr glPoints_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; - mrpt::opengl::CSetOfObjects::Ptr gl_sensor_fov_; + mrpt::viz::CPointCloudColoured::Ptr glPoints_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_origin_, gl_sensor_origin_corner_; + mrpt::viz::CSetOfObjects::Ptr gl_sensor_fov_; std::optional has_to_render_; std::mutex has_to_render_mtx_; diff --git a/modules/simulator/include/mvsim/Sensors/SensorBase.h b/modules/simulator/include/mvsim/Sensors/SensorBase.h index 247d6139..c6a689d9 100644 --- a/modules/simulator/include/mvsim/Sensors/SensorBase.h +++ b/modules/simulator/include/mvsim/Sensors/SensorBase.h @@ -10,8 +10,8 @@ #pragma once #include -#include #include +#include #include #include #include @@ -31,7 +31,7 @@ class Simulable; * * \ingroup sensors_module */ -class SensorBase : public VisualObject, public Simulable +class SensorBase : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; @@ -50,17 +50,14 @@ class SensorBase : public VisualObject, public Simulable void registerOnServer(mvsim::Client& c) override; - virtual void simulateOn3DScene( // - [[maybe_unused]] mrpt::opengl::COpenGLScene& gl_scene) - { - } + virtual void simulateOn3DScene([[maybe_unused]] mrpt::viz::Scene& gl_scene) {} // Get all sensors visuals API: - static std::shared_ptr GetAllSensorsOriginViz(); + static std::shared_ptr GetAllSensorsOriginViz(); - static std::shared_ptr GetAllSensorsFOVViz(); - static void RegisterSensorFOVViz(const std::shared_ptr& o); - static void RegisterSensorOriginViz(const std::shared_ptr& o); + static std::shared_ptr GetAllSensorsFOVViz(); + static void RegisterSensorFOVViz(const std::shared_ptr& o); + static void RegisterSensorOriginViz(const std::shared_ptr& o); double sensor_period() const { return sensor_period_; } diff --git a/modules/simulator/include/mvsim/Shape2p5.h b/modules/simulator/include/mvsim/Shape2p5.h index cbc815e1..a7bdbe8f 100644 --- a/modules/simulator/include/mvsim/Shape2p5.h +++ b/modules/simulator/include/mvsim/Shape2p5.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include @@ -35,7 +35,7 @@ class Shape2p5 void buildInit( const mrpt::math::TPoint2Df& bbMin, const mrpt::math::TPoint2Df& bbMax, int numCells = 100); void buildAddPoint(const mrpt::math::TPoint3Df& pt); - void buildAddTriangle(const mrpt::opengl::TTriangle& t); + void buildAddTriangle(const mrpt::viz::TTriangle& t); const mrpt::math::TPolygon2D& getContour() const; diff --git a/modules/simulator/include/mvsim/Simulable.h b/modules/simulator/include/mvsim/Simulable.h index 745ff21c..78c4154c 100644 --- a/modules/simulator/include/mvsim/Simulable.h +++ b/modules/simulator/include/mvsim/Simulable.h @@ -22,7 +22,7 @@ namespace mvsim { class Client; class World; -class VisualObject; +class CVisualObject; /** \ingroup mvsim_simulator_module */ struct ParseSimulableParams @@ -61,7 +61,7 @@ class Simulable const mrpt::math::TVector2D& force, const mrpt::math::TPoint2D& applyPoint = mrpt::math::TPoint2D(0, 0)); - virtual VisualObject* meAsVisualObject() { return nullptr; } + virtual CVisualObject* meAsCVisualObject() { return nullptr; } /** Last time-step velocity (of the reference point, in local coords) * Note this is converted from Box2D center of mass velocity into our own reference point. diff --git a/modules/simulator/include/mvsim/VehicleBase.h b/modules/simulator/include/mvsim/VehicleBase.h index b3031859..d926f787 100644 --- a/modules/simulator/include/mvsim/VehicleBase.h +++ b/modules/simulator/include/mvsim/VehicleBase.h @@ -14,9 +14,9 @@ #include #include #include -#include -#include #include +#include +#include #include #include #include @@ -40,7 +40,7 @@ namespace mvsim * * \ingroup virtual_interfaces_module */ -class VehicleBase : public VisualObject, public Simulable +class VehicleBase : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; @@ -186,8 +186,8 @@ class VehicleBase : public VisualObject, public Simulable virtual void initLoggers(); virtual void writeLogStrings(); virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; protected: // Protected ctor for class factory @@ -204,7 +204,7 @@ class VehicleBase : public VisualObject, public Simulable { } - VisualObject* meAsVisualObject() override { return this; } + CVisualObject* meAsCVisualObject() override { return this; } /** When true, friction forces are computed (for wheel spin updates and * logging) but NOT applied to the Box2D chassis body. This is set by @@ -285,15 +285,15 @@ class VehicleBase : public VisualObject, public Simulable private: // Called from internalGuiUpdate() void internal_internalGuiUpdate_sensors( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical); + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical); // Called from internalGuiUpdate() - void internal_internalGuiUpdate_forces(mrpt::opengl::COpenGLScene& scene); + void internal_internalGuiUpdate_forces(mrpt::viz::Scene& scene); - mrpt::opengl::CSetOfObjects::Ptr glChassisViz_, glChassisPhysical_; - std::vector glWheelsViz_, glWheelsPhysical_; - mrpt::opengl::CSetOfLines::Ptr glForces_; - mrpt::opengl::CSetOfLines::Ptr glMotorTorques_; + mrpt::viz::CSetOfObjects::Ptr glChassisViz_, glChassisPhysical_; + std::vector glWheelsViz_, glWheelsPhysical_; + mrpt::viz::CSetOfLines::Ptr glForces_; + mrpt::viz::CSetOfLines::Ptr glMotorTorques_; std::atomic_bool glInit_ = false; std::vector forceSegmentsForRendering_; diff --git a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h index 494d061d..81170e1d 100644 --- a/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h +++ b/modules/simulator/include/mvsim/VehicleDynamics/VehicleAckermann.h @@ -9,6 +9,7 @@ #pragma once +#include #include #include #include diff --git a/modules/simulator/include/mvsim/VisualObject.h b/modules/simulator/include/mvsim/VisualObject.h index a7b9407f..b28414d2 100644 --- a/modules/simulator/include/mvsim/VisualObject.h +++ b/modules/simulator/include/mvsim/VisualObject.h @@ -11,9 +11,9 @@ #include #include -#include -#include #include +#include +#include #include #include @@ -32,26 +32,26 @@ class World; * sent out to RViz) * \ingroup virtual_interfaces_module */ -class VisualObject +class CVisualObject { public: - VisualObject( + CVisualObject( World* parent, bool insertCustomVizIntoViz = true, bool insertCustomVizIntoPhysical = true); - virtual ~VisualObject(); + virtual ~CVisualObject(); - VisualObject(const VisualObject&) = delete; - VisualObject& operator=(const VisualObject&) = delete; - VisualObject(VisualObject&&) = delete; - VisualObject& operator=(VisualObject&&) = delete; + CVisualObject(const CVisualObject&) = delete; + CVisualObject& operator=(const CVisualObject&) = delete; + CVisualObject(CVisualObject&&) = delete; + CVisualObject& operator=(CVisualObject&&) = delete; /** This creates a new object in the scene and/or update it according to the * current state of the object. If none of the scenes are passed, the poses * of existing visual objects are updated, but no new ones are created. */ virtual void guiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical); + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical); World* parent() { return world_; } const World* parent() const { return world_; } @@ -79,20 +79,19 @@ class VisualObject /** If not empty, will override the derived-class visualization for this * object. */ - std::shared_ptr glCustomVisual_; - std::shared_ptr glCollision_; + std::shared_ptr glCustomVisual_; + std::shared_ptr glCollision_; int32_t glCustomVisualId_ = -1; const bool insertCustomVizIntoViz_ = true; const bool insertCustomVizIntoPhysical_ = true; virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - bool childrenOnly = false) = 0; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly = false) = 0; void addCustomVisualization( - const mrpt::opengl::CRenderizable::Ptr& glModel, const mrpt::poses::CPose3D& modelPose = {}, + const mrpt::viz::CVisualObject::Ptr& glModel, const mrpt::poses::CPose3D& modelPose = {}, const float modelScale = 1.0f, const std::string& modelName = "group", const std::optional& modelURI = std::nullopt, const bool initialShowBoundingBox = false, diff --git a/modules/simulator/include/mvsim/Wheel.h b/modules/simulator/include/mvsim/Wheel.h index 9d4bdc41..71329971 100644 --- a/modules/simulator/include/mvsim/Wheel.h +++ b/modules/simulator/include/mvsim/Wheel.h @@ -30,7 +30,7 @@ class World; * * \ingroup mvsim_simulator_module */ -class Wheel : public VisualObject +class Wheel : public CVisualObject { public: Wheel(World* world); @@ -75,12 +75,12 @@ class Wheel : public VisualObject // methods ---- - void getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene); + void getAs3DObject(mrpt::viz::CSetOfObjects& obj, bool isPhysicalScene); void loadFromXML(const rapidxml::xml_node* xml_node); void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; double getPhi() const { return phi; } //!< Orientation (rad) wrt vehicle local frame void setPhi(double val) { phi = val; } //!< Orientation (rad) wrt vehicle local frame diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 41c90cb4..a00d6756 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -252,7 +253,7 @@ class World : public mrpt::system::COutputLogger * There are two sets of objects: "viz" for visualization only, "physical" * for objects which should be detected by sensors. */ - mrpt::opengl::CSetOfObjects::Ptr guiUserObjectsPhysical_, guiUserObjectsViz_; + mrpt::viz::CSetOfObjects::Ptr guiUserObjectsPhysical_, guiUserObjectsViz_; std::mutex guiUserObjectsMtx_; /// Update 3D vehicles, sensors, run render-based sensors, etc: @@ -260,10 +261,9 @@ class World : public mrpt::system::COutputLogger /// mode. void internalGraphicsLoopTasksForSimulation(); - void internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene& physicalObjects); + void internalRunSensorsOn3DScene(mrpt::viz::Scene& physicalObjects); - void internalUpdate3DSceneObjects( - mrpt::opengl::COpenGLScene& viz, mrpt::opengl::COpenGLScene& physical); + void internalUpdate3DSceneObjects(mrpt::viz::Scene& viz, mrpt::viz::Scene& physical); void internal_GUI_thread(); void internal_process_pending_gui_user_tasks(); @@ -272,22 +272,18 @@ class World : public mrpt::system::COutputLogger void mark_as_pending_running_sensors_on_3D_scene() { - pendingRunSensorsOn3DSceneMtx_.lock(); + std::lock_guard lck(pendingRunSensorsOn3DSceneMtx_); pendingRunSensorsOn3DScene_ = true; - pendingRunSensorsOn3DSceneMtx_.unlock(); } void clear_pending_running_sensors_on_3D_scene() { - pendingRunSensorsOn3DSceneMtx_.lock(); + std::lock_guard lck(pendingRunSensorsOn3DSceneMtx_); pendingRunSensorsOn3DScene_ = false; - pendingRunSensorsOn3DSceneMtx_.unlock(); } bool pending_running_sensors_on_3D_scene() { - pendingRunSensorsOn3DSceneMtx_.lock(); - bool ret = pendingRunSensorsOn3DScene_; - pendingRunSensorsOn3DSceneMtx_.unlock(); - return ret; + std::lock_guard lck(pendingRunSensorsOn3DSceneMtx_); + return pendingRunSensorsOn3DScene_; } std::string guiMsgLines_; @@ -301,23 +297,19 @@ class World : public mrpt::system::COutputLogger bool simulator_must_close() const { - gui_thread_start_mtx_.lock(); - const bool v = simulator_must_close_; - gui_thread_start_mtx_.unlock(); - return v; + std::lock_guard lck(gui_thread_start_mtx_); + return simulator_must_close_; } void simulator_must_close(bool value) { - gui_thread_start_mtx_.lock(); + std::lock_guard lck(gui_thread_start_mtx_); simulator_must_close_ = value; - gui_thread_start_mtx_.unlock(); } void enqueue_task_to_run_in_gui_thread(const std::function& f) { - guiUserPendingTasksMtx_.lock(); + std::lock_guard lck(guiUserPendingTasksMtx_); guiUserPendingTasks_.emplace_back(f); - guiUserPendingTasksMtx_.unlock(); } std::vector> guiUserPendingTasks_; @@ -557,8 +549,8 @@ class World : public mrpt::system::COutputLogger /// This private container will be filled with objects in the public /// gui_user_objects_ - mrpt::opengl::CSetOfObjects::Ptr glUserObjsPhysical_ = mrpt::opengl::CSetOfObjects::Create(); - mrpt::opengl::CSetOfObjects::Ptr glUserObjsViz_ = mrpt::opengl::CSetOfObjects::Create(); + mrpt::viz::CSetOfObjects::Ptr glUserObjsPhysical_ = mrpt::viz::CSetOfObjects::Create(); + mrpt::viz::CSetOfObjects::Ptr glUserObjsViz_ = mrpt::viz::CSetOfObjects::Create(); // ------- GUI options ----- struct TGUI_Options @@ -628,11 +620,21 @@ class World : public mrpt::system::COutputLogger float shadow_bias_normal = 1e-4; mrpt::img::TColor light_color = {0xff, 0xff, 0xff, 0xff}; - float light_ambient = 0.5f; + float light_ambient = 0.4f; + float light_diffuse = 0.8f; + float light_specular = 0.6f; + + /// Hemisphere ambient sky color (surfaces facing up) + mrpt::img::TColor ambient_sky_color = {0xe0, 0xe8, 0xff, 0xff}; + /// Hemisphere ambient ground color (surfaces facing down) + mrpt::img::TColor ambient_ground_color = {0x40, 0x3a, 0x30, 0xff}; float eye_distance_to_shadow_map_extension = 2.0f; //!< [m/m] float minimum_shadow_map_extension_ratio = 0.005f; //!< [0,1] + /** Additional light sources (point and spot) parsed from XML */ + std::vector extra_lights; + const TParameterDefinitions params = { {"enable_shadows", {"%bool", &enable_shadows}}, {"shadow_map_size", {"%i", &shadow_map_size}}, @@ -641,10 +643,14 @@ class World : public mrpt::system::COutputLogger {"light_clip_plane_min", {"%f", &light_clip_plane_min}}, {"light_clip_plane_max", {"%f", &light_clip_plane_max}}, {"light_color", {"%color", &light_color}}, + {"light_diffuse", {"%f", &light_diffuse}}, + {"light_specular", {"%f", &light_specular}}, {"shadow_bias", {"%f", &shadow_bias}}, {"shadow_bias_cam2frag", {"%f", &shadow_bias_cam2frag}}, {"shadow_bias_normal", {"%f", &shadow_bias_normal}}, {"light_ambient", {"%f", &light_ambient}}, + {"ambient_sky_color", {"%color", &ambient_sky_color}}, + {"ambient_ground_color", {"%color", &ambient_ground_color}}, {"eye_distance_to_shadow_map_extension", {"%f", &eye_distance_to_shadow_map_extension}}, {"minimum_shadow_map_extension_ratio", {"%f", &minimum_shadow_map_extension_ratio}}, }; @@ -815,7 +821,7 @@ class World : public mrpt::system::COutputLogger { nanogui::CheckBox* cb = nullptr; Simulable::Ptr simulable; - VisualObject* visual = nullptr; + CVisualObject* visual = nullptr; }; // Buttons that must be {dis,en}abled when there is a selected object: @@ -839,14 +845,14 @@ class World : public mrpt::system::COutputLogger /** 3D scene with all visual objects (vehicles, obstacles, markers, etc.) * \sa worldPhysical_ */ - mrpt::opengl::COpenGLScene::Ptr worldVisual_ = mrpt::opengl::COpenGLScene::Create(); + mrpt::viz::Scene::Ptr worldVisual_ = mrpt::viz::Scene::Create(); /** 3D scene with all physically observable objects: we will use this * scene as input to simulated sensors like cameras, where we don't wont * to see visualization marks, etc. * \sa world_visual_ */ - mrpt::opengl::COpenGLScene worldPhysical_; + mrpt::viz::Scene worldPhysical_; std::recursive_mutex worldPhysicalMtx_; /// World coordinates offset for rendering. Useful mainly to keep numerical accuracy diff --git a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h index e63f7835..5a1795b9 100644 --- a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h +++ b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h @@ -10,7 +10,7 @@ #pragma once #include -#include +#include #include #include @@ -45,12 +45,12 @@ class ElevationMap : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; /** This object holds both, the mesh data, and is in charge of 3D rendering. */ - std::vector gl_meshes_; + std::vector gl_meshes_; bool firstSceneRendering_ = true; double resolution_ = 1.0f; diff --git a/modules/simulator/include/mvsim/WorldElements/GroundGrid.h b/modules/simulator/include/mvsim/WorldElements/GroundGrid.h index f6d469af..f4ebec74 100644 --- a/modules/simulator/include/mvsim/WorldElements/GroundGrid.h +++ b/modules/simulator/include/mvsim/WorldElements/GroundGrid.h @@ -10,7 +10,7 @@ #pragma once #include -#include +#include #include namespace mvsim @@ -29,8 +29,8 @@ class GroundGrid : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; bool is_floating_; std::string float_center_at_vehicle_name_; @@ -38,6 +38,6 @@ class GroundGrid : public WorldElementBase mrpt::img::TColor color_; double line_width_; - mrpt::opengl::CGridPlaneXY::Ptr gl_groundgrid_; + mrpt::viz::CGridPlaneXY::Ptr gl_groundgrid_; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h b/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h index 657421da..d9d02b71 100644 --- a/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h +++ b/modules/simulator/include/mvsim/WorldElements/HorizontalPlane.h @@ -10,9 +10,9 @@ #pragma once #include -#include -#include -#include +#include +#include +#include #include namespace mvsim @@ -25,7 +25,6 @@ class HorizontalPlane : public WorldElementBase DECLARES_REGISTER_WORLD_ELEMENT(HorizontalPlane) public: HorizontalPlane(World* parent, const rapidxml::xml_node* root); - virtual ~HorizontalPlane(); virtual void loadConfigFrom(const rapidxml::xml_node* root) override; // ------- Interface with "World" ------ @@ -36,8 +35,8 @@ class HorizontalPlane : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; float x_min_ = -10, x_max_ = 10, y_min_ = -10, y_max_ = 10; mrpt::img::TColor color_ = {0xa0, 0xa0, 0xa0, 0xff}; @@ -51,8 +50,8 @@ class HorizontalPlane : public WorldElementBase float z_ = .0f; std::string cull_faces_ = "NONE"; - mrpt::opengl::CTexturedPlane::Ptr gl_plane_; - mrpt::opengl::CSetOfTexturedTriangles::Ptr gl_plane_text_; - mrpt::opengl::CSetOfObjects::Ptr glGroup_; + mrpt::viz::CTexturedPlane::Ptr gl_plane_; + mrpt::viz::CSetOfTexturedTriangles::Ptr gl_plane_text_; + mrpt::viz::CSetOfObjects::Ptr glGroup_; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h b/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h index fbd81ebd..be2f1817 100644 --- a/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h +++ b/modules/simulator/include/mvsim/WorldElements/OccupancyGridMap.h @@ -12,9 +12,9 @@ #include #include #include -#include -#include #include +#include +#include #include #include @@ -46,14 +46,14 @@ class OccupancyGridMap : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; mrpt::maps::COccupancyGridMap2D grid_; bool gui_uptodate_; //!< Whether gl_grid_ has to be updated upon next //! call of internalGuiUpdate() - mrpt::opengl::CSetOfObjects::Ptr gl_grid_; + mrpt::viz::CSetOfObjects::Ptr gl_grid_; struct TFixturePtr { @@ -73,10 +73,10 @@ class OccupancyGridMap : public WorldElementBase }; std::vector obstacles_for_each_obj_; - std::vector gl_obs_clouds_; + std::vector gl_obs_clouds_; std::mutex gl_obs_clouds_buffer_cs_; - std::vector gl_obs_clouds_buffer_; + std::vector gl_obs_clouds_buffer_; mrpt::obs::CSinCosLookUpTableFor2DScans sincos_lut_; diff --git a/modules/simulator/include/mvsim/WorldElements/PointCloud.h b/modules/simulator/include/mvsim/WorldElements/PointCloud.h index afca3719..85a0eb82 100644 --- a/modules/simulator/include/mvsim/WorldElements/PointCloud.h +++ b/modules/simulator/include/mvsim/WorldElements/PointCloud.h @@ -10,8 +10,8 @@ #pragma once #include -#include -#include +#include +#include #include #include @@ -43,15 +43,15 @@ class PointCloud : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; mrpt::maps::CPointsMap::Ptr points_; /// Whether gl_grid_ has to be updated upon next call of /// internalGuiUpdate() bool gui_uptodate_ = false; - mrpt::opengl::CSetOfObjects::Ptr gl_points_; + mrpt::viz::CSetOfObjects::Ptr gl_points_; double render_points_size_ = 3.0; mrpt::poses::CPose3D pointcloud_pose_ = mrpt::poses::CPose3D::Identity(); diff --git a/modules/simulator/include/mvsim/WorldElements/PropertyRegion.h b/modules/simulator/include/mvsim/WorldElements/PropertyRegion.h index 14246f2c..a6a60b98 100644 --- a/modules/simulator/include/mvsim/WorldElements/PropertyRegion.h +++ b/modules/simulator/include/mvsim/WorldElements/PropertyRegion.h @@ -42,8 +42,8 @@ class PropertyRegion : public WorldElementBase virtual void simul_pre_timestep(const TSimulContext&) override {} virtual void simul_post_timestep(const TSimulContext&) override {} virtual void internalGuiUpdate( - const mrpt::optional_ref&, - const mrpt::optional_ref&, bool) override + const mrpt::optional_ref&, const mrpt::optional_ref&, + bool) override { } diff --git a/modules/simulator/include/mvsim/WorldElements/SkyBox.h b/modules/simulator/include/mvsim/WorldElements/SkyBox.h index c79ea3dd..5a4fad79 100644 --- a/modules/simulator/include/mvsim/WorldElements/SkyBox.h +++ b/modules/simulator/include/mvsim/WorldElements/SkyBox.h @@ -9,7 +9,7 @@ #pragma once -#include +#include #include namespace mvsim @@ -31,14 +31,14 @@ class SkyBox : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; - // It holds a CSkyBox object. Stored as base CRenderizable to prevent + // It holds a CSkyBox object. Stored as base CVisualObject to prevent // depending in this public header on mrpt >=2.7.0 so mvsim can still be // built with older mrpt versions. - mrpt::opengl::CRenderizable::Ptr glSkyBox_; + mrpt::viz::CVisualObject::Ptr glSkyBox_; - mrpt::opengl::CRenderizable::Ptr glSkyBoxPrepared_; + mrpt::viz::CVisualObject::Ptr glSkyBoxPrepared_; }; } // namespace mvsim diff --git a/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h b/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h index e85e7244..54fb9dbb 100644 --- a/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h +++ b/modules/simulator/include/mvsim/WorldElements/VerticalPlane.h @@ -10,9 +10,9 @@ #pragma once #include -#include -#include -#include +#include +#include +#include #include namespace mvsim @@ -55,16 +55,16 @@ class VerticalPlane : public WorldElementBase protected: virtual void internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) override; + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) override; /** Create Box2D physics bodies for the wall segments */ void createPhysicsBodies(); /** Create visual representation with proper segmentation for openings */ void createVisualRepresentation( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical); + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical); /** Render a front/back face of the wall */ void renderWallFace( @@ -100,9 +100,9 @@ class VerticalPlane : public WorldElementBase float z_ = .0f, height_ = 3.0f; std::string cull_faces_ = "NONE"; - mrpt::opengl::CTexturedPlane::Ptr gl_plane_; - mrpt::opengl::CSetOfTexturedTriangles::Ptr gl_plane_text_; - mrpt::opengl::CSetOfObjects::Ptr glGroup_; + mrpt::viz::CTexturedPlane::Ptr gl_plane_; + mrpt::viz::CSetOfTexturedTriangles::Ptr gl_plane_text_; + mrpt::viz::CSetOfObjects::Ptr glGroup_; /** Box2D body for the wall (may be split into multiple fixtures) */ b2Body* b2dBody_ = nullptr; diff --git a/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h b/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h index 89876555..2afaf895 100644 --- a/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h +++ b/modules/simulator/include/mvsim/WorldElements/WorldElementBase.h @@ -24,12 +24,12 @@ namespace mvsim * \ingroup world_elements_module * */ -class WorldElementBase : public VisualObject, public Simulable +class WorldElementBase : public CVisualObject, public Simulable { public: using Ptr = std::shared_ptr; - WorldElementBase(World* parent) : VisualObject(parent), Simulable(parent) {} + WorldElementBase(World* parent) : CVisualObject(parent), Simulable(parent) {} virtual ~WorldElementBase() {} WorldElementBase(const WorldElementBase&) = delete; diff --git a/modules/simulator/src/Block.cpp b/modules/simulator/src/Block.cpp index a183d68b..57298468 100644 --- a/modules/simulator/src/Block.cpp +++ b/modules/simulator/src/Block.cpp @@ -11,13 +11,13 @@ #include #include #include -#include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include #include #include @@ -35,7 +35,7 @@ using namespace std; static XmlClassesRegistry block_classes_registry("block:class"); // Protected ctor: -Block::Block(World* parent) : VisualObject(parent), Simulable(parent) +Block::Block(World* parent) : CVisualObject(parent), Simulable(parent) { // Default shape: block_poly_.emplace_back(-0.5, -0.5); @@ -233,8 +233,8 @@ void Block::simul_post_timestep(const TSimulContext& context) } void Block::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) { // 1st time call?? -> Create objects // ---------------------------------- @@ -242,17 +242,17 @@ void Block::internalGuiUpdate( { if (!gl_block_ && viz && physical) { - gl_block_ = mrpt::opengl::CSetOfObjects::Create(); + gl_block_ = mrpt::viz::CSetOfObjects::Create(); gl_block_->setName(name_); // Block shape: - auto gl_poly = mrpt::opengl::CPolyhedron::CreateCustomPrism( - block_poly_, block_z_max_ - block_z_min_); + auto gl_poly = + mrpt::viz::CPolyhedron::CreateCustomPrism(block_poly_, block_z_max_ - block_z_min_); gl_poly->setLocation(0, 0, block_z_min_); gl_poly->setColor_u8(block_color_); // Hide back faces: - // gl_poly->cullFaces(mrpt::opengl::TCullFace::FRONT); + // gl_poly->cullFaces(mrpt::viz::TCullFace::FRONT); gl_block_->insert(gl_poly); @@ -275,7 +275,7 @@ void Block::internalGuiUpdate( if (!gl_forces_ && viz) { // Visualization of forces: - gl_forces_ = mrpt::opengl::CSetOfLines::Create(); + gl_forces_ = mrpt::viz::CSetOfLines::Create(); gl_forces_->setLineWidth(3.0); gl_forces_->setColor_u8(0xff, 0xff, 0xff); @@ -292,7 +292,7 @@ void Block::internalGuiUpdate( } void Block::internal_internalGuiUpdate_forces( // - [[maybe_unused]] mrpt::opengl::COpenGLScene& scene) + [[maybe_unused]] mrpt::viz::Scene& scene) { if (world_->guiOptions_.show_forces) { @@ -448,12 +448,13 @@ void Block::setIsStatic(bool b) } // Protected ctor: -DummyInvisibleBlock::DummyInvisibleBlock(World* parent) : VisualObject(parent), Simulable(parent) {} +DummyInvisibleBlock::DummyInvisibleBlock(World* parent) : CVisualObject(parent), Simulable(parent) +{ +} void DummyInvisibleBlock::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { if (!viz || !physical) { @@ -490,7 +491,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node if (_.vertex_count == 0) _.vertex_count = 10; // default - auto glCyl = mrpt::opengl::CCylinder::Create(); + auto glCyl = mrpt::viz::CCylinder::Create(); glCyl->setHeight(_.length); glCyl->setRadius(_.radius); glCyl->setSlicesCount(_.vertex_count); @@ -505,7 +506,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node if (_.vertex_count == 0) _.vertex_count = 10; // default - auto glSph = mrpt::opengl::CSphere::Create(_.radius, _.vertex_count); + auto glSph = mrpt::viz::CSphere::Create(_.radius, _.vertex_count); glSph->setColor_u8(block_color_); addCustomVisualization(glSph); } @@ -517,7 +518,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node ASSERTMSG_(_.ly > 0, "Missing 'ly' attribute for box geometry"); ASSERTMSG_(_.lz > 0, "Missing 'lz' attribute for box geometry"); - auto glBox = mrpt::opengl::CBox::Create(); + auto glBox = mrpt::viz::CBox::Create(); glBox->setBoxCorners({0, 0, 0}, {_.lx, _.ly, _.lz}); glBox->setColor_u8(block_color_); addCustomVisualization(glBox); @@ -532,7 +533,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node if (_.vertex_count == 0) _.vertex_count = 10; // default - auto glCyl = mrpt::opengl::CCylinder::Create(); + auto glCyl = mrpt::viz::CCylinder::Create(); glCyl->setHeight(_.lx); glCyl->setRadius(_.ly * 0.5); glCyl->setScale(2 * _.lz / _.ly, 1.0, 1.0); @@ -552,7 +553,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node ASSERTMSG_(_.ly > 0, "Missing 'ly' attribute for ramp geometry"); ASSERTMSG_(_.lz > 0, "Missing 'lz' attribute for ramp geometry"); - auto glRamp = mrpt::opengl::CSetOfTriangles::Create(); + auto glRamp = mrpt::viz::CSetOfTriangles::Create(); const auto p0 = mrpt::math::TPoint3Df(0, -_.ly * 0.5, 0); const auto p1 = mrpt::math::TPoint3Df(_.lx, -_.ly * 0.5, _.lz); @@ -561,7 +562,7 @@ void Block::internal_parseGeometry(const rapidxml::xml_node& xml_geom_node const auto p4 = mrpt::math::TPoint3Df(_.lx, -_.ly * 0.5, 0); const auto p5 = mrpt::math::TPoint3Df(_.lx, _.ly * 0.5, 0); - mrpt::opengl::TTriangle t; + mrpt::viz::TTriangle t; t.setColor(block_color_); // T0 t.vertex(0) = p0; diff --git a/modules/simulator/src/CollisionShapeCache.cpp b/modules/simulator/src/CollisionShapeCache.cpp index 40ac0073..cecfabee 100644 --- a/modules/simulator/src/CollisionShapeCache.cpp +++ b/modules/simulator/src/CollisionShapeCache.cpp @@ -10,11 +10,12 @@ #include // b2_maxPolygonVertices #include #include -#include -#include -#include -#include +#include #include +#include +#include +#include +#include #include using namespace mvsim; @@ -26,7 +27,7 @@ CollisionShapeCache& CollisionShapeCache::Instance() } Shape2p5 CollisionShapeCache::get( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, + mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale, const std::optional& modelFile) { // already cached? @@ -82,12 +83,12 @@ Shape2p5 CollisionShapeCache::get( } std::optional CollisionShapeCache::processSimpleGeometries( - const mrpt::opengl::CRenderizable& obj, float zMin, float zMax, + const mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale) { using mrpt::literals::operator""_deg; - if (auto oCyl = dynamic_cast(&obj); oCyl) + if (auto oCyl = dynamic_cast(&obj); oCyl) { // =============================== // Cylinder @@ -105,7 +106,7 @@ std::optional CollisionShapeCache::processSimpleGeometries( actualEdgeCount, actualRadius, zMin, zMax, modelPose, modelScale); } - if (auto oSph = dynamic_cast(&obj); oSph) + if (auto oSph = dynamic_cast(&obj); oSph) { // =============================== // Sphere @@ -118,7 +119,7 @@ std::optional CollisionShapeCache::processSimpleGeometries( actualEdgeCount, actualRadius, zMin, zMax, modelPose, modelScale); } - if (auto oBox = dynamic_cast(&obj); oBox) + if (auto oBox = dynamic_cast(&obj); oBox) { // =============================== // Box @@ -154,38 +155,25 @@ std::optional CollisionShapeCache::processSimpleGeometries( } Shape2p5 CollisionShapeCache::processGenericGeometry( - mrpt::opengl::CRenderizable& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, + mrpt::viz::CVisualObject& obj, float zMin, float zMax, const mrpt::poses::CPose3D& modelPose, const float modelScale) { Shape2p5 ret; // Make sure the points and vertices buffers are up to date, so we can // access them: - auto* oAssimp = dynamic_cast(&obj); + auto* oAssimp = dynamic_cast(&obj); if (oAssimp) { - oAssimp->onUpdateBuffers_all(); - } - auto* oRSWF = dynamic_cast(&obj); - if (oRSWF) - { - oRSWF->onUpdateBuffers_Wireframe(); - } - auto* oRST = dynamic_cast(&obj); - if (oRST) - { - oRST->onUpdateBuffers_Triangles(); - } - auto* oRSTT = dynamic_cast(&obj); - if (oRSTT) - { - oRSTT->onUpdateBuffers_TexturedTriangles(); - } - auto* oRP = dynamic_cast(&obj); - if (oRP) - { - oRP->onUpdateBuffers_Points(); + oAssimp->updateBuffers(); } + auto* oRSWF = dynamic_cast(&obj); + auto* oRST = dynamic_cast(&obj); + auto* oRSTT = dynamic_cast(&obj); + auto* oRP = dynamic_cast(&obj); + + // Ensure all buffers are up to date: + obj.updateBuffers(); // Slice bbox in z up to a given relevant height: size_t numTotalPts = 0, numPassedPts = 0; @@ -210,11 +198,11 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( ret.buildAddPoint(pt); numPassedPts++; }; - auto lambdaUpdateTri = [&](const mrpt::opengl::TTriangle& tri) + auto lambdaUpdateTri = [&](const mrpt::viz::TTriangle& tri) { numTotalPts += 3; // transform the whole triangle, then compare with [z,z] limits: - mrpt::opengl::TTriangle t = tri; + mrpt::viz::TTriangle t = tri; for (int i = 0; i < 3; i++) t.vertex(i) = modelPose.composePoint(t.vertex(i) * modelScale); // does any of the point lie within the valid Z range, or is the @@ -275,8 +263,8 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( } if (oRSWF) { - auto lck = mrpt::lockHelper(oRSWF->shaderWireframeBuffersMutex().data); - const auto& pts = oRSWF->shaderWireframeVertexPointBuffer(); + auto lck = mrpt::lockHelper(oRSWF->shaderLinesBufferMutex().data); + const auto& pts = oRSWF->shaderLinesVertexPointBuffer(); for (const auto& pt : pts) { lambdaUpdatePt(pt); @@ -285,19 +273,28 @@ Shape2p5 CollisionShapeCache::processGenericGeometry( if (oAssimp) { - const auto& txtrdObjs = oAssimp->texturedObjects(); // mrpt>=2.6.0 - for (const auto& o : txtrdObjs) + // CAssimpModel is a CSetOfObjects in mrpt3; iterate children for meshes: + for (const auto& child : *oAssimp) { - if (!o) + if (!child) continue; + + // Textured triangles: + if (auto* tt = + dynamic_cast(child.get())) { - continue; + auto lck = mrpt::lockHelper(tt->shaderTexturedTrianglesBufferMutex().data); + const auto& tris = tt->shaderTexturedTrianglesBuffer(); + for (const auto& tri : tris) + lambdaUpdateTri(tri); } - auto lck = mrpt::lockHelper(o->shaderTexturedTrianglesBufferMutex().data); - const auto& tris = o->shaderTexturedTrianglesBuffer(); - for (const auto& tri : tris) + // Non-textured triangles: + if (auto* st = dynamic_cast(child.get())) { - lambdaUpdateTri(tri); + auto lck = mrpt::lockHelper(st->shaderTrianglesBufferMutex().data); + const auto& tris = st->shaderTrianglesBuffer(); + for (const auto& tri : tris) + lambdaUpdateTri(tri); } } } diff --git a/modules/simulator/src/FrictionModels/EllipseCurveMethod.cpp b/modules/simulator/src/FrictionModels/EllipseCurveMethod.cpp index 01bd0d6a..0cd6442a 100644 --- a/modules/simulator/src/FrictionModels/EllipseCurveMethod.cpp +++ b/modules/simulator/src/FrictionModels/EllipseCurveMethod.cpp @@ -10,9 +10,9 @@ #include #include -#include #include #include +#include #include // For use as default model #include #include diff --git a/modules/simulator/src/HumanActor.cpp b/modules/simulator/src/HumanActor.cpp index a4d4ee28..31a2ce79 100644 --- a/modules/simulator/src/HumanActor.cpp +++ b/modules/simulator/src/HumanActor.cpp @@ -29,7 +29,7 @@ static XmlClassesRegistry actor_classes_registry("actor:class"); // Constructor // ============================================================================ -HumanActor::HumanActor(World* parent) : VisualObject(parent), Simulable(parent) {} +HumanActor::HumanActor(World* parent) : CVisualObject(parent), Simulable(parent) {} // ============================================================================ // Class Registration @@ -553,47 +553,10 @@ void HumanActor::updateSkeletalAnimation(double dt) currentAnimTime_ += dt * animSpeedScale; -#if MRPT_VERSION >= MIN_MRPT_VERSION_ANIMATED_ASSIMP - // Drive the CAnimatedAssimpModel if we have one. - // Lazy discovery: walk the visual hierarchy on first use. - if (glCustomVisual_ && !glModel_) - { - // The hierarchy built by VisualObject::addCustomVisualization is: - // glCustomVisual_ -> CSetOfObjects("group") -> CAssimpModel - // We look for a CAnimatedAssimpModel at any depth. - for (auto& obj : *glCustomVisual_) - { - // Direct child? - glModel_ = std::dynamic_pointer_cast(obj); - if (glModel_) - { - break; - } - // Inside a CSetOfObjects wrapper? - if (auto grp = std::dynamic_pointer_cast(obj); grp) - { - for (auto& inner : *grp) - { - glModel_ = std::dynamic_pointer_cast(inner); - if (glModel_) - { - break; - } - } - } - if (glModel_) - { - break; - } - } - } - - if (glModel_) - { - glModel_->setActiveAnimation(animName); - glModel_->setAnimationTime(currentAnimTime_); - } -#endif + // Store current animation name for the GUI thread to apply. + // Do NOT call setAnimationTime() here — that triggers rebuildSkinnedGeometry() + // which modifies CSetOfObjects children. That must only happen on the GUI thread. + currentAnimName_ = animName; } // ============================================================================ @@ -601,8 +564,8 @@ void HumanActor::updateSkeletalAnimation(double dt) // ============================================================================ void HumanActor::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) { // On first call, if no was defined, create a simple // placeholder capsule so the actor is visible. @@ -610,17 +573,17 @@ void HumanActor::internalGuiUpdate( { if (!glCustomVisual_) { - auto cylinder = mrpt::opengl::CCylinder::Create( + auto cylinder = mrpt::viz::CCylinder::Create( static_cast(collisionRadius_), static_cast(collisionRadius_), static_cast(collisionHeight_)); cylinder->setColor_u8(mrpt::img::TColor(100, 150, 200, 200)); cylinder->setLocation(0, 0, 0); - auto head = mrpt::opengl::CSphere::Create(static_cast(collisionRadius_ * 0.8)); + auto head = mrpt::viz::CSphere::Create(static_cast(collisionRadius_ * 0.8)); head->setColor_u8(mrpt::img::TColor(220, 180, 150)); head->setLocation(0, 0, static_cast(collisionHeight_)); - auto placeholder = mrpt::opengl::CSetOfObjects::Create(); + auto placeholder = mrpt::viz::CSetOfObjects::Create(); placeholder->insert(cylinder); placeholder->insert(head); @@ -631,11 +594,43 @@ void HumanActor::internalGuiUpdate( } // Everything else (scene insertion, pose update) is handled by the - // base class VisualObject::guiUpdate(), which calls us here. + // base class CVisualObject::guiUpdate(), which calls us here. // We deliberately do NOT duplicate pose-update logic. (void)viz; (void)physical; (void)childrenOnly; + +#if MRPT_VERSION >= MIN_MRPT_VERSION_ANIMATED_ASSIMP + // Apply animation state to the model on the GUI thread. + // This MUST happen here (GUI thread), not in the simulation thread, because + // setAnimationTime() calls rebuildSkinnedGeometry() which modifies CSetOfObjects + // children — a data race if done from the sim thread while the GUI thread iterates. + + // Lazy discovery of the CAnimatedAssimpModel in the visual hierarchy. + if (glCustomVisual_ && !glModel_) + { + for (auto& obj : *glCustomVisual_) + { + glModel_ = std::dynamic_pointer_cast(obj); + if (glModel_) break; + if (auto grp = std::dynamic_pointer_cast(obj); grp) + { + for (auto& inner : *grp) + { + glModel_ = std::dynamic_pointer_cast(inner); + if (glModel_) break; + } + } + if (glModel_) break; + } + } + + if (glModel_ && !currentAnimName_.empty()) + { + glModel_->setActiveAnimation(currentAnimName_); + glModel_->setAnimationTime(currentAnimTime_); + } +#endif } // ============================================================================ @@ -657,19 +652,19 @@ void HumanActor::upgradeToAnimatedModel() } // Walk the visual hierarchy to find the CAssimpModel and its wrapper. - mrpt::opengl::CAssimpModel::Ptr foundModel; - mrpt::opengl::CSetOfObjects::Ptr containingGroup; + mrpt::viz::CAssimpModel::Ptr foundModel; + mrpt::viz::CSetOfObjects::Ptr containingGroup; for (auto& obj : *glCustomVisual_) { - auto grp = std::dynamic_pointer_cast(obj); + auto grp = std::dynamic_pointer_cast(obj); if (!grp) { continue; } for (auto& inner : *grp) { - auto model = std::dynamic_pointer_cast(inner); + auto model = std::dynamic_pointer_cast(inner); if (model) { foundModel = model; @@ -690,12 +685,12 @@ void HumanActor::upgradeToAnimatedModel() // Create a dedicated CAnimatedAssimpModel and load from file. #if MRPT_VERSION >= MIN_MRPT_VERSION_ANIMATED_ASSIMP - auto animModel = mrpt::opengl::CAnimatedAssimpModel::Create(); + auto animModel = mrpt::viz::CAnimatedAssimpModel::Create(); #else - auto animModel = mrpt::opengl::CAssimpModel::Create(); + auto animModel = mrpt::viz::CAssimpModel::Create(); #endif - const int loadFlags = mrpt::opengl::CAssimpModel::LoadFlags::RealTimeMaxQuality | - mrpt::opengl::CAssimpModel::LoadFlags::FlipUVs; + const int loadFlags = mrpt::viz::CAssimpModel::LoadFlags::RealTimeMaxQuality | + mrpt::viz::CAssimpModel::LoadFlags::FlipUVs; try { diff --git a/modules/simulator/src/JointXMLnode.h b/modules/simulator/src/JointXMLnode.h index 330d253d..429a583e 100644 --- a/modules/simulator/src/JointXMLnode.h +++ b/modules/simulator/src/JointXMLnode.h @@ -33,7 +33,10 @@ class JointXMLnode for (const auto& node : nodes_) { ret = node->first_node(name); - if (ret != nullptr) return ret; + if (ret != nullptr) + { + return ret; + } } return ret; } @@ -51,29 +54,42 @@ class JointXMLnode // ++it iterator& operator++() { - if (!current) throw std::runtime_error("++ called on end() iterator!?"); + if (!current) + { + throw std::runtime_error("++ called on end() iterator!?"); + } current = current->next_sibling(); JointXMLnode::TListNodes& lst = parent.getListOfNodes(); while (!current && lst_idx < lst.size()) { lst_idx++; if (lst_idx < lst.size()) + { current = lst[lst_idx]->first_node(); + } else + { current = nullptr; + } } return *this; } rapidxml::xml_node* operator->() const { - if (!current) throw std::runtime_error("-> called on end() iterator!?"); + if (!current) + { + throw std::runtime_error("-> called on end() iterator!?"); + } return current; } rapidxml::xml_node* operator*() const { - if (!current) throw std::runtime_error("* called on end() iterator!?"); + if (!current) + { + throw std::runtime_error("* called on end() iterator!?"); + } return current; } @@ -92,7 +108,10 @@ class JointXMLnode while (!current && lst_idx < lst.size()) { current = lst[lst_idx]->first_node(); - if (!current) lst_idx++; + if (!current) + { + lst_idx++; + } } } // end() diff --git a/modules/simulator/src/Joystick.cpp b/modules/simulator/src/Joystick.cpp index 069c3544..93ecef80 100644 --- a/modules/simulator/src/Joystick.cpp +++ b/modules/simulator/src/Joystick.cpp @@ -18,7 +18,7 @@ | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ -#include +#include #include #ifdef _WIN32 diff --git a/modules/simulator/src/ModelsCache.cpp b/modules/simulator/src/ModelsCache.cpp index 3ec0d3fe..31c85b71 100644 --- a/modules/simulator/src/ModelsCache.cpp +++ b/modules/simulator/src/ModelsCache.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -24,7 +25,7 @@ ModelsCache& ModelsCache::Instance() return o; } -mrpt::opengl::CAssimpModel::Ptr ModelsCache::get( +mrpt::viz::CAssimpModel::Ptr ModelsCache::get( const std::string& localFileName, const Options& options) { // already cached? @@ -34,31 +35,43 @@ mrpt::opengl::CAssimpModel::Ptr ModelsCache::get( } // No, it's a new model path, create its placeholder: - auto m = cache[localFileName] = mrpt::opengl::CAssimpModel::Create(); + auto m = cache[localFileName] = mrpt::viz::CAssimpModel::Create(); ASSERT_FILE_EXISTS_(localFileName); // En/Dis-able the extra verbosity while loading the 3D model: - int loadFlags = mrpt::opengl::CAssimpModel::LoadFlags::RealTimeMaxQuality | - mrpt::opengl::CAssimpModel::LoadFlags::FlipUVs; + int loadFlags = mrpt::viz::CAssimpModel::LoadFlags::RealTimeMaxQuality | + mrpt::viz::CAssimpModel::LoadFlags::FlipUVs; if (options.modelColor != mrpt::img::TColor::white()) { - loadFlags |= mrpt::opengl::CAssimpModel::LoadFlags::IgnoreMaterialColor; + loadFlags |= mrpt::viz::CAssimpModel::LoadFlags::IgnoreMaterialColor; } m->setColor_u8(options.modelColor); if (mrpt::get_env("MVSIM_LOAD_MODELS_VERBOSE", false)) { - loadFlags |= mrpt::opengl::CAssimpModel::LoadFlags::Verbose; + loadFlags |= mrpt::viz::CAssimpModel::LoadFlags::Verbose; } m->loadScene(localFileName, loadFlags); - m->cullFaces(mrpt::typemeta::TEnumType::name2value(options.modelCull)); + { + const auto cf = + mrpt::typemeta::TEnumType::name2value(options.modelCull); + for (auto& child : *m) + { + if (!child) continue; + if (auto* t = dynamic_cast(child.get())) + t->cullFaces(cf); + if (auto* tt = + dynamic_cast(child.get())) + tt->cullFaces(cf); + } + } - m->split_triangles_rendering_bbox(options.splitSize); + m->setSplitTrianglesRenderingBBox(options.splitSize); return m; } diff --git a/modules/simulator/src/ModelsCache.h b/modules/simulator/src/ModelsCache.h index 8bd91662..3cea68c9 100644 --- a/modules/simulator/src/ModelsCache.h +++ b/modules/simulator/src/ModelsCache.h @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include @@ -23,14 +23,14 @@ class ModelsCache mrpt::img::TColor modelColor = mrpt::img::TColor::white(); std::string modelCull = "NONE"; - /** See mrpt::opengl::CAssimpModel::split_triangles_rendering_bbox(). + /** See mrpt::viz::CAssimpModel::split_triangles_rendering_bbox(). * Default (0)=disabled. Any other value, split the model into voxels of this size * to help sorting triangles by depth so semitransparent meshes are rendered correctly. */ float splitSize = .0f; }; - mrpt::opengl::CAssimpModel::Ptr get(const std::string& url, const Options& options); + mrpt::viz::CAssimpModel::Ptr get(const std::string& url, const Options& options); void clear() { cache.clear(); } @@ -38,7 +38,12 @@ class ModelsCache ModelsCache() = default; ~ModelsCache() = default; - std::map cache; + ModelsCache(const ModelsCache&) = default; + ModelsCache& operator=(const ModelsCache&) = default; + ModelsCache(ModelsCache&&) = default; + ModelsCache& operator=(ModelsCache&&) = default; + + std::map cache; }; } // namespace mvsim diff --git a/modules/simulator/src/Sensors/CameraSensor.cpp b/modules/simulator/src/Sensors/CameraSensor.cpp index 3d86a03f..0d69ca1d 100644 --- a/modules/simulator/src/Sensors/CameraSensor.cpp +++ b/modules/simulator/src/Sensors/CameraSensor.cpp @@ -8,11 +8,11 @@ +-------------------------------------------------------------------------+ */ #include -#include -#include -#include #include #include +#include +#include +#include #include #include #include @@ -81,15 +81,15 @@ void CameraSensor::loadConfigFrom(const rapidxml::xml_node* root) } void CameraSensor::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -99,7 +99,7 @@ void CameraSensor::internalGuiUpdate( } if (!gl_sensor_fov_ && viz) { - gl_sensor_fov_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_fov_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_fov_->setVisibility(false); viz->get().insert(gl_sensor_fov_); SensorBase::RegisterSensorFOVViz(gl_sensor_fov_); @@ -115,11 +115,11 @@ void CameraSensor::internalGuiUpdate( if (!gl_sensor_frustum_) { - gl_sensor_frustum_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_frustum_ = mrpt::viz::CSetOfObjects::Create(); const float frustumScale = 0.4e-3; auto frustum = - mrpt::opengl::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); + mrpt::viz::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); gl_sensor_frustum_->insert(frustum); gl_sensor_fov_->insert(gl_sensor_frustum_); @@ -153,7 +153,7 @@ void CameraSensor::internalGuiUpdate( void CameraSensor::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} -void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) +void CameraSensor::simulateOn3DScene(mrpt::viz::Scene& world3DScene) { using namespace mrpt; // _deg @@ -191,7 +191,8 @@ void CameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) auto viewport = world3DScene.getViewport(); - auto& cam = fbo_renderer_rgb_->getCamera(world3DScene); + if (!fbo_renderer_rgb_->hasCameraOverride()) fbo_renderer_rgb_->setCamera(mrpt::viz::CCamera()); + auto& cam = fbo_renderer_rgb_->getCameraOverride(); const auto fixedAxisConventionRot = mrpt::poses::CPose3D(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg); diff --git a/modules/simulator/src/Sensors/DepthCameraSensor.cpp b/modules/simulator/src/Sensors/DepthCameraSensor.cpp index 8af33387..c8fb27fe 100644 --- a/modules/simulator/src/Sensors/DepthCameraSensor.cpp +++ b/modules/simulator/src/Sensors/DepthCameraSensor.cpp @@ -8,11 +8,11 @@ +-------------------------------------------------------------------------+ */ #include -#include -#include -#include #include #include +#include +#include +#include #include #include #include @@ -112,14 +112,14 @@ void DepthCameraSensor::loadConfigFrom(const rapidxml::xml_node* root) } void DepthCameraSensor::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { - mrpt::opengl::CSetOfObjects::Ptr glVizSensors; + mrpt::viz::CSetOfObjects::Ptr glVizSensors; if (viz) { - glVizSensors = std::dynamic_pointer_cast( + glVizSensors = std::dynamic_pointer_cast( viz->get().getByName("group_sensors_viz")); if (!glVizSensors) return; // may happen during shutdown } @@ -127,7 +127,7 @@ void DepthCameraSensor::internalGuiUpdate( // 1st time? if (!gl_obs_ && glVizSensors) { - gl_obs_ = mrpt::opengl::CPointCloudColoured::Create(); + gl_obs_ = mrpt::viz::CPointCloudColoured::Create(); gl_obs_->setPointSize(2.0f); gl_obs_->setLocalRepresentativePoint(sensor_params_.sensorPose.translation()); glVizSensors->insert(gl_obs_); @@ -135,9 +135,9 @@ void DepthCameraSensor::internalGuiUpdate( if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -147,7 +147,7 @@ void DepthCameraSensor::internalGuiUpdate( } if (!gl_sensor_fov_ && viz) { - gl_sensor_fov_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_fov_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_fov_->setVisibility(false); viz->get().insert(gl_sensor_fov_); SensorBase::RegisterSensorFOVViz(gl_sensor_fov_); @@ -171,11 +171,11 @@ void DepthCameraSensor::internalGuiUpdate( if (!gl_sensor_frustum_) { - gl_sensor_frustum_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_frustum_ = mrpt::viz::CSetOfObjects::Create(); const float frustumScale = 0.4e-3; auto frustum = - mrpt::opengl::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); + mrpt::viz::CFrustum::Create(last_obs2gui_->cameraParams, frustumScale); gl_sensor_frustum_->insert(frustum); gl_sensor_fov_->insert(gl_sensor_frustum_); @@ -215,7 +215,7 @@ void DepthCameraSensor::internalGuiUpdate( void DepthCameraSensor::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) {} -void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) +void DepthCameraSensor::simulateOn3DScene(mrpt::viz::Scene& world3DScene) { using namespace mrpt; // _deg @@ -274,8 +274,16 @@ void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DSce auto viewport = world3DScene.getViewport(); - auto* camDepth = fbo_renderer_depth_ ? &fbo_renderer_depth_->getCamera(world3DScene) : nullptr; - auto* camRGB = fbo_renderer_rgb_ ? &fbo_renderer_rgb_->getCamera(world3DScene) : nullptr; + if (fbo_renderer_depth_) + { + if (!fbo_renderer_depth_->hasCameraOverride()) + fbo_renderer_depth_->setCamera(mrpt::viz::CCamera()); + } + if (fbo_renderer_rgb_) + { + if (!fbo_renderer_rgb_->hasCameraOverride()) + fbo_renderer_rgb_->setCamera(mrpt::viz::CCamera()); + } const auto fixedAxisConventionRot = mrpt::poses::CPose3D(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg); @@ -302,9 +310,10 @@ void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DSce auto tle2 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderRGB"); - camRGB->set6DOFMode(true); - camRGB->setProjectiveFromPinhole(curObs.cameraParamsIntensity); - camRGB->setPose(world()->applyWorldRenderOffset(rgbSensorPose)); + auto& camRGB = fbo_renderer_rgb_->getCameraOverride(); + camRGB.set6DOFMode(true); + camRGB.setProjectiveFromPinhole(curObs.cameraParamsIntensity); + camRGB.setPose(world()->applyWorldRenderOffset(rgbSensorPose)); // viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f}); viewport->setViewportClipDistances(rgbClipMin_, rgbClipMax_); @@ -325,13 +334,14 @@ void DepthCameraSensor::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DSce { auto tle2 = mrpt::system::CTimeLoggerEntry(world_->getTimeLogger(), "sensor.RGBD.renderD"); - camDepth->setProjectiveFromPinhole(curObs.cameraParams); + auto& camDepth = fbo_renderer_depth_->getCameraOverride(); + camDepth.setProjectiveFromPinhole(curObs.cameraParams); // Camera pose: vehicle + relativePoseOnVehicle: // Note: relativePoseOnVehicle should be (y,p,r)=(90deg,0,90deg) to make // the camera to look forward: - camDepth->set6DOFMode(true); - camDepth->setPose(world()->applyWorldRenderOffset(depthSensorPose)); + camDepth.set6DOFMode(true); + camDepth.setPose(world()->applyWorldRenderOffset(depthSensorPose)); // viewport->setCustomBackgroundColor({0.3f, 0.3f, 0.3f, 1.0f}); viewport->setViewportClipDistances(depth_clip_min_, depth_clip_max_); diff --git a/modules/simulator/src/Sensors/GNSS.cpp b/modules/simulator/src/Sensors/GNSS.cpp index d0fb3cb9..15905465 100644 --- a/modules/simulator/src/Sensors/GNSS.cpp +++ b/modules/simulator/src/Sensors/GNSS.cpp @@ -8,9 +8,9 @@ +-------------------------------------------------------------------------+ */ #include -#include #include #include +#include #include #include #include @@ -58,16 +58,16 @@ void GNSS::loadConfigFrom(const rapidxml::xml_node* root) } void GNSS::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { // 1st time? if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); diff --git a/modules/simulator/src/Sensors/IMU.cpp b/modules/simulator/src/Sensors/IMU.cpp index dde0b88f..0fe144e4 100644 --- a/modules/simulator/src/Sensors/IMU.cpp +++ b/modules/simulator/src/Sensors/IMU.cpp @@ -8,9 +8,9 @@ +-------------------------------------------------------------------------+ */ #include -#include #include #include +#include #include #include #include @@ -69,16 +69,16 @@ void IMU::loadConfigFrom(const rapidxml::xml_node* root) } void IMU::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { // 1st time? if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); diff --git a/modules/simulator/src/Sensors/LaserScanner.cpp b/modules/simulator/src/Sensors/LaserScanner.cpp index 44816c67..96979213 100644 --- a/modules/simulator/src/Sensors/LaserScanner.cpp +++ b/modules/simulator/src/Sensors/LaserScanner.cpp @@ -8,10 +8,10 @@ +-------------------------------------------------------------------------+ */ #include -#include -#include #include #include +#include +#include #include #include #include @@ -83,16 +83,16 @@ void LaserScanner::loadConfigFrom(const rapidxml::xml_node* root) } void LaserScanner::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace std::string_literals; - mrpt::opengl::CSetOfObjects::Ptr glVizSensors; + mrpt::viz::CSetOfObjects::Ptr glVizSensors; if (viz) { - glVizSensors = std::dynamic_pointer_cast( + glVizSensors = std::dynamic_pointer_cast( viz->get().getByName("group_sensors_viz")); if (!glVizSensors) return; // may happen during shutdown } @@ -100,7 +100,7 @@ void LaserScanner::internalGuiUpdate( // 1st time? if (!gl_scan_ && glVizSensors) { - gl_scan_ = mrpt::opengl::CPlanarLaserScan::Create(); + gl_scan_ = mrpt::viz::CPlanarLaserScan::Create(); gl_scan_->enablePoints(viz_visiblePoints_); gl_scan_->setPointSize(viz_pointSize_); @@ -120,9 +120,9 @@ void LaserScanner::internalGuiUpdate( } if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -132,10 +132,10 @@ void LaserScanner::internalGuiUpdate( } if (!gl_sensor_fov_ && viz) { - gl_sensor_fov_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_fov_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_fov_->castShadows(false); - auto fovScan = mrpt::opengl::CPlanarLaserScan::Create(); + auto fovScan = mrpt::viz::CPlanarLaserScan::Create(); fovScan->enablePoints(false); fovScan->enableSurface(true); @@ -445,7 +445,7 @@ void LaserScanner::freeOpenGLResources() fbo_renderer_depth_.reset(); } -void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) +void LaserScanner::simulateOn3DScene(mrpt::viz::Scene& world3DScene) { using namespace mrpt; // _deg @@ -503,7 +503,9 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) auto viewport = world3DScene.getViewport(); - auto& cam = fbo_renderer_depth_->getCamera(world3DScene); + if (!fbo_renderer_depth_->hasCameraOverride()) + fbo_renderer_depth_->setCamera(mrpt::viz::CCamera()); + auto& cam = fbo_renderer_depth_->getCameraOverride(); const auto fixedAxisConventionRot = mrpt::poses::CPose3D(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg); @@ -568,7 +570,7 @@ void LaserScanner::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) mrpt::math::CMatrixFloat depthImage; // make owner's own body invisible? - auto visVeh = dynamic_cast(&vehicle_); + auto visVeh = dynamic_cast(&vehicle_); auto veh = dynamic_cast(&vehicle_); bool formerVisVehState = true; if (ignore_parent_body_) diff --git a/modules/simulator/src/Sensors/Lidar3D.cpp b/modules/simulator/src/Sensors/Lidar3D.cpp index 7bfe97cf..ffa41441 100644 --- a/modules/simulator/src/Sensors/Lidar3D.cpp +++ b/modules/simulator/src/Sensors/Lidar3D.cpp @@ -10,9 +10,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -66,14 +66,14 @@ void Lidar3D::loadConfigFrom(const rapidxml::xml_node* root) } void Lidar3D::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { - mrpt::opengl::CSetOfObjects::Ptr glVizSensors; + mrpt::viz::CSetOfObjects::Ptr glVizSensors; if (viz) { - glVizSensors = std::dynamic_pointer_cast( + glVizSensors = std::dynamic_pointer_cast( viz->get().getByName("group_sensors_viz")); if (!glVizSensors) { @@ -84,7 +84,7 @@ void Lidar3D::internalGuiUpdate( // 1st time? if (!glPoints_ && glVizSensors) { - glPoints_ = mrpt::opengl::CPointCloudColoured::Create(); + glPoints_ = mrpt::viz::CPointCloudColoured::Create(); glPoints_->setPointSize(viz_pointSize_); glPoints_->setLocalRepresentativePoint({0, 0, 0.10f}); @@ -92,9 +92,9 @@ void Lidar3D::internalGuiUpdate( } if (!gl_sensor_origin_ && viz) { - gl_sensor_origin_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_origin_ = mrpt::viz::CSetOfObjects::Create(); gl_sensor_origin_->castShadows(false); - gl_sensor_origin_corner_ = mrpt::opengl::stock_objects::CornerXYZSimple(0.15f); + gl_sensor_origin_corner_ = mrpt::viz::stock_objects::CornerXYZSimple(0.15f); gl_sensor_origin_->insert(gl_sensor_origin_corner_); @@ -104,10 +104,10 @@ void Lidar3D::internalGuiUpdate( } if (!gl_sensor_fov_ && viz) { - gl_sensor_fov_ = mrpt::opengl::CSetOfObjects::Create(); + gl_sensor_fov_ = mrpt::viz::CSetOfObjects::Create(); // Create lines to represent the FOV, as a "360 deg" frustum: - auto fovLines = mrpt::opengl::CSetOfLines::Create(); + auto fovLines = mrpt::viz::CSetOfLines::Create(); const float fovRange = 1.0f; // unit sphere const size_t N_LINES = 32; const double ang_vert_min = -0.5 * mrpt::DEG2RAD(vertical_fov_); @@ -307,7 +307,7 @@ static float safeInterpolateRangeImage( return d00; } -void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) +void Lidar3D::simulateOn3DScene(mrpt::viz::Scene& world3DScene) { using namespace mrpt; // _deg @@ -451,7 +451,9 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) const bool wasShadowEnabled = viewport->isShadowCastingEnabled(); viewport->enableShadowCasting(false); - auto& cam = fbo_renderer_depth_->getCamera(world3DScene); + if (!fbo_renderer_depth_->hasCameraOverride()) + fbo_renderer_depth_->setCamera(mrpt::viz::CCamera()); + auto& cam = fbo_renderer_depth_->getCameraOverride(); const auto fixedAxisConventionRot = mrpt::poses::CPose3D(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg); @@ -552,7 +554,7 @@ void Lidar3D::simulateOn3DScene(mrpt::opengl::COpenGLScene& world3DScene) mrpt::img::CImage convergenceRgbImage; // make owner's own body invisible? - auto visVeh = dynamic_cast(&vehicle_); + auto visVeh = dynamic_cast(&vehicle_); auto veh = dynamic_cast(&vehicle_); bool formerVisVehState = true; if (ignore_parent_body_) diff --git a/modules/simulator/src/Sensors/SensorBase.cpp b/modules/simulator/src/Sensors/SensorBase.cpp index bb2d368f..14f9cd72 100644 --- a/modules/simulator/src/Sensors/SensorBase.cpp +++ b/modules/simulator/src/Sensors/SensorBase.cpp @@ -54,35 +54,35 @@ void register_all_sensors() REGISTER_SENSOR("gnss", GNSS) } -static auto gAllSensorsOriginViz = mrpt::opengl::CSetOfObjects::Create(); -static auto gAllSensorsFOVViz = mrpt::opengl::CSetOfObjects::Create(); +static auto gAllSensorsOriginViz = mrpt::viz::CSetOfObjects::Create(); +static auto gAllSensorsFOVViz = mrpt::viz::CSetOfObjects::Create(); static std::mutex gAllSensorVizMtx; -mrpt::opengl::CSetOfObjects::Ptr SensorBase::GetAllSensorsOriginViz() +mrpt::viz::CSetOfObjects::Ptr SensorBase::GetAllSensorsOriginViz() { auto lck = mrpt::lockHelper(gAllSensorVizMtx); return gAllSensorsOriginViz; } -mrpt::opengl::CSetOfObjects::Ptr SensorBase::GetAllSensorsFOVViz() +mrpt::viz::CSetOfObjects::Ptr SensorBase::GetAllSensorsFOVViz() { auto lck = mrpt::lockHelper(gAllSensorVizMtx); return gAllSensorsFOVViz; } -void SensorBase::RegisterSensorFOVViz(const mrpt::opengl::CSetOfObjects::Ptr& o) +void SensorBase::RegisterSensorFOVViz(const mrpt::viz::CSetOfObjects::Ptr& o) { auto lck = mrpt::lockHelper(gAllSensorVizMtx); gAllSensorsFOVViz->insert(o); } -void SensorBase::RegisterSensorOriginViz(const mrpt::opengl::CSetOfObjects::Ptr& o) +void SensorBase::RegisterSensorOriginViz(const mrpt::viz::CSetOfObjects::Ptr& o) { auto lck = mrpt::lockHelper(gAllSensorVizMtx); gAllSensorsOriginViz->insert(o); } SensorBase::SensorBase(Simulable& vehicle) - : VisualObject(vehicle.getSimulableWorldObject()), + : CVisualObject(vehicle.getSimulableWorldObject()), Simulable(vehicle.getSimulableWorldObject()), vehicle_(vehicle) { diff --git a/modules/simulator/src/Shape2p5.cpp b/modules/simulator/src/Shape2p5.cpp index dd6beaad..43f872e2 100644 --- a/modules/simulator/src/Shape2p5.cpp +++ b/modules/simulator/src/Shape2p5.cpp @@ -14,12 +14,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -69,7 +69,7 @@ void Shape2p5::mergeWith(const Shape2p5& s) const auto p0 = ca.at(im1); const auto p1 = ca.at(i); - mrpt::opengl::TTriangle t; + mrpt::viz::TTriangle t; t.vertex(0) = TPoint3Df(p0.x, p0.y, bb.min.z); t.vertex(1) = TPoint3Df(p1.x, p1.y, bb.min.z); t.vertex(2) = TPoint3Df(p1.x, p1.y, bb.max.z); @@ -81,7 +81,7 @@ void Shape2p5::mergeWith(const Shape2p5& s) const auto p0 = cb.at(im1); const auto p1 = cb.at(i); - mrpt::opengl::TTriangle t; + mrpt::viz::TTriangle t; t.vertex(0) = TPoint3Df(p0.x, p0.y, bb.min.z); t.vertex(1) = TPoint3Df(p1.x, p1.y, bb.min.z); t.vertex(2) = TPoint3Df(p1.x, p1.y, bb.max.z); @@ -100,7 +100,7 @@ const mrpt::math::TPolygon2D& Shape2p5::getContour() const // For debugging only: #ifdef DEBUG_DUMP_TRIANGLES -static auto glDebugTriangles = mrpt::opengl::CSetOfTriangles::Create(); +static auto glDebugTriangles = mrpt::viz::CSetOfTriangles::Create(); #endif void Shape2p5::buildInit( @@ -132,7 +132,7 @@ void Shape2p5::buildAddPoint(const mrpt::math::TPoint3Df& pt) *c = CELL_OCCUPIED; } -void Shape2p5::buildAddTriangle(const mrpt::opengl::TTriangle& t) +void Shape2p5::buildAddTriangle(const mrpt::viz::TTriangle& t) { const float step = grid_->getResolution(); @@ -207,7 +207,7 @@ void Shape2p5::computeShape() const #ifdef DEBUG_DUMP_TRIANGLES { static int cnt = 0; - mrpt::opengl::COpenGLScene scene; + mrpt::viz::Scene scene; scene.insert(glDebugTriangles); scene.saveToFile(mrpt::format("debug_shape2p5_triangles_%04i.3Dscene", cnt++)); } @@ -509,9 +509,9 @@ mrpt::math::TPolygon2D Shape2p5::internalGridContour() const void Shape2p5::debugSaveGridTo3DSceneFile( const mrpt::math::TPolygon2D& rawGridContour, const std::string& debugStr) const { - mrpt::opengl::COpenGLScene scene; + mrpt::viz::Scene scene; - auto glGrid = mrpt::opengl::CTexturedPlane::Create(); + auto glGrid = mrpt::viz::CTexturedPlane::Create(); glGrid->setPlaneCorners(grid_->getXMin(), grid_->getXMax(), grid_->getYMin(), grid_->getYMax()); mrpt::math::CMatrixDouble mat; @@ -522,14 +522,14 @@ void Shape2p5::debugSaveGridTo3DSceneFile( glGrid->assignImage(im); - scene.insert(mrpt::opengl::stock_objects::CornerXYZSimple()); + scene.insert(mrpt::viz::stock_objects::CornerXYZSimple()); scene.insert(glGrid); auto lambdaRenderPoly = [&scene](const mrpt::math::TPolygon2D& p, const mrpt::img::TColor& color, double z) { - auto glPts = mrpt::opengl::CPointCloud::Create(); - auto glPoly = mrpt::opengl::CSetOfLines::Create(); + auto glPts = mrpt::viz::CPointCloud::Create(); + auto glPoly = mrpt::viz::CSetOfLines::Create(); glPoly->setColor_u8(color); glPts->setColor_u8(color); glPts->setPointSize(4.0f); diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp index 644c48a5..7fcbcbc4 100644 --- a/modules/simulator/src/Simulable.cpp +++ b/modules/simulator/src/Simulable.cpp @@ -87,7 +87,7 @@ void Simulable::simul_post_timestep(const TSimulContext& context) // world-element modifies them! (e.g. elevation map) // Update the GUI element **poses** only: - if (auto* vo = meAsVisualObject(); vo) + if (auto* vo = meAsCVisualObject(); vo) { vo->guiUpdate(std::nullopt, std::nullopt); } @@ -560,7 +560,7 @@ void Simulable::setPose(const mrpt::math::TPose3D& p, bool notifyChange) const me.q_ = p; // Update the GUI element poses only: - if (auto* vo = me.meAsVisualObject(); vo) + if (auto* vo = me.meAsCVisualObject(); vo) { vo->guiUpdate(std::nullopt, std::nullopt); } diff --git a/modules/simulator/src/VehicleBase.cpp b/modules/simulator/src/VehicleBase.cpp index 7a999bc3..49b2fdd5 100644 --- a/modules/simulator/src/VehicleBase.cpp +++ b/modules/simulator/src/VehicleBase.cpp @@ -9,10 +9,10 @@ #include #include -#include #include #include #include +#include #include // For use as default model #include #include @@ -59,7 +59,7 @@ void register_all_veh_dynamics() // Protected ctor: VehicleBase::VehicleBase(World* parent, size_t nWheels) - : VisualObject(parent), Simulable(parent), fixture_wheels_(nWheels, nullptr) + : CVisualObject(parent), Simulable(parent), fixture_wheels_(nWheels, nullptr) { // Create wheels: for (size_t i = 0; i < nWheels; i++) @@ -681,8 +681,8 @@ std::vector VehicleBase::getWheelsVelocityLocal( } void VehicleBase::internal_internalGuiUpdate_sensors( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical) { for (auto& s : sensors_) { @@ -691,7 +691,7 @@ void VehicleBase::internal_internalGuiUpdate_sensors( } void VehicleBase::internal_internalGuiUpdate_forces( // - [[maybe_unused]] mrpt::opengl::COpenGLScene& scene) + [[maybe_unused]] mrpt::viz::Scene& scene) { if (world_->guiOptions_.show_forces) { @@ -847,18 +847,18 @@ void VehicleBase::create_multibody_system(b2World& world) } void VehicleBase::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, bool childrenOnly) { // 1st time call?? -> Create objects // ---------------------------------- const size_t nWs = this->getNumWheels(); if (!glChassisViz_ && viz && physical) { - glChassisViz_ = mrpt::opengl::CSetOfObjects::Create(); + glChassisViz_ = mrpt::viz::CSetOfObjects::Create(); glChassisViz_->setName("vehicle_chassis_"s + name_); - glChassisPhysical_ = mrpt::opengl::CSetOfObjects::Create(); + glChassisPhysical_ = mrpt::viz::CSetOfObjects::Create(); glChassisPhysical_->setName("vehicle_chassis_"s + name_); // Wheels shape: @@ -866,11 +866,11 @@ void VehicleBase::internalGuiUpdate( glWheelsPhysical_.resize(nWs); for (size_t i = 0; i < nWs; i++) { - glWheelsViz_[i] = mrpt::opengl::CSetOfObjects::Create(); + glWheelsViz_[i] = mrpt::viz::CSetOfObjects::Create(); this->getWheelInfo(i).getAs3DObject(*glWheelsViz_[i], false); glChassisViz_->insert(glWheelsViz_[i]); - glWheelsPhysical_[i] = mrpt::opengl::CSetOfObjects::Create(); + glWheelsPhysical_[i] = mrpt::viz::CSetOfObjects::Create(); this->getWheelInfo(i).getAs3DObject(*glWheelsPhysical_[i], true); glChassisPhysical_->insert(glWheelsPhysical_[i]); } @@ -878,7 +878,7 @@ void VehicleBase::internalGuiUpdate( if (!childrenOnly) { // Robot shape: - auto gl_poly = mrpt::opengl::CPolyhedron::CreateCustomPrism( + auto gl_poly = mrpt::viz::CPolyhedron::CreateCustomPrism( chassis_poly_, chassis_z_max_ - chassis_z_min_); gl_poly->setLocation(0, 0, chassis_z_min_); gl_poly->setColor_u8(chassis_color_); @@ -914,7 +914,7 @@ void VehicleBase::internalGuiUpdate( if (!w.linked_yaw_object_name.empty()) { - auto glLinked = VisualObject::glCustomVisual_->getByName(w.linked_yaw_object_name); + auto glLinked = CVisualObject::glCustomVisual_->getByName(w.linked_yaw_object_name); if (!glLinked) { THROW_EXCEPTION_FMT( @@ -934,7 +934,7 @@ void VehicleBase::internalGuiUpdate( if (!glForces_ && viz) { // Visualization of forces: - glForces_ = mrpt::opengl::CSetOfLines::Create(); + glForces_ = mrpt::viz::CSetOfLines::Create(); glForces_->setLineWidth(3.0); glForces_->setColor_u8(0xff, 0xff, 0xff); glForces_->setPose(parent()->applyWorldRenderOffset(mrpt::poses::CPose3D::Identity())); @@ -944,7 +944,7 @@ void VehicleBase::internalGuiUpdate( if (!glMotorTorques_ && viz) { // Visualization of forces: - glMotorTorques_ = mrpt::opengl::CSetOfLines::Create(); + glMotorTorques_ = mrpt::viz::CSetOfLines::Create(); glMotorTorques_->setLineWidth(3.0); glMotorTorques_->setColor_u8(0xff, 0x00, 0x00); glMotorTorques_->setPose( diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp index 46b7fdf6..003ad079 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann.cpp @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include #include diff --git a/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp b/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp index b150944e..491bb1e1 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleAckermann_Drivetrain.cpp @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include #include diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp index a98ce58b..11bbec3b 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential.cpp @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include #include diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp index 39089444..8380cdec 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistIdeal.cpp @@ -7,6 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ +#include #include using namespace mvsim; diff --git a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp index 8111f424..a340f684 100644 --- a/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp +++ b/modules/simulator/src/VehicleDynamics/VehicleDifferential_ControllerTwistPID.cpp @@ -8,6 +8,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ +#include #include #include "xml_utils.h" diff --git a/modules/simulator/src/VisualObject.cpp b/modules/simulator/src/VisualObject.cpp index 170e95bf..2d9d2cf8 100644 --- a/modules/simulator/src/VisualObject.cpp +++ b/modules/simulator/src/VisualObject.cpp @@ -7,10 +7,10 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include -#include -#include #include +#include +#include +#include #include #include #include @@ -27,23 +27,23 @@ using namespace mvsim; static std::atomic_int32_t g_uniqueCustomVisualId = 0; -double VisualObject::GeometryEpsilon = 1e-3; +double CVisualObject::GeometryEpsilon = 1e-3; -VisualObject::VisualObject( +CVisualObject::CVisualObject( World* parent, bool insertCustomVizIntoViz, bool insertCustomVizIntoPhysical) : world_(parent), insertCustomVizIntoViz_(insertCustomVizIntoViz), insertCustomVizIntoPhysical_(insertCustomVizIntoPhysical) { - glCollision_ = mrpt::opengl::CSetOfObjects::Create(); + glCollision_ = mrpt::viz::CSetOfObjects::Create(); glCollision_->setName("bbox"); } -VisualObject::~VisualObject() = default; +CVisualObject::~CVisualObject() = default; -void VisualObject::guiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical) +void CVisualObject::guiUpdate( + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical) { using namespace std::string_literals; @@ -109,16 +109,16 @@ void VisualObject::guiUpdate( } mrpt::math::setEpsilon(1e-5 * smallestEdge); - mrpt::opengl::CPolyhedron::Ptr glCS; + mrpt::viz::CPolyhedron::Ptr glCS; try { - glCS = mrpt::opengl::CPolyhedron::CreateCustomPrism(c, height); + glCS = mrpt::viz::CPolyhedron::CreateCustomPrism(c, height); } catch (const std::exception& e) { #if 0 - std::cerr << "[mvsim::VisualObject] **WARNING**: Ignoring the " + std::cerr << "[mvsim::CVisualObject] **WARNING**: Ignoring the " "following error while building the visualization " "of the collision shape for object named '" << meSim->getName() @@ -135,7 +135,7 @@ void VisualObject::guiUpdate( p.emplace_back(bbMin.x, bbMax.y); p.emplace_back(bbMax.x, bbMax.y); p.emplace_back(bbMax.x, bbMin.y); - glCS = mrpt::opengl::CPolyhedron::CreateCustomPrism(p, height); + glCS = mrpt::viz::CPolyhedron::CreateCustomPrism(p, height); } glCS->setWireframe(true); @@ -156,9 +156,9 @@ void VisualObject::guiUpdate( internalGuiUpdate(viz, physical, childrenOnly); } -void VisualObject::FreeOpenGLResources() { ModelsCache::Instance().clear(); } +void CVisualObject::FreeOpenGLResources() { ModelsCache::Instance().clear(); } -bool VisualObject::parseVisual(const rapidxml::xml_node& rootNode) +bool CVisualObject::parseVisual(const rapidxml::xml_node& rootNode) { MRPT_TRY_START @@ -173,7 +173,7 @@ bool VisualObject::parseVisual(const rapidxml::xml_node& rootNode) MRPT_TRY_END } -bool VisualObject::parseVisual(const JointXMLnode<>& rootNode) +bool CVisualObject::parseVisual(const JointXMLnode<>& rootNode) { MRPT_TRY_START @@ -188,7 +188,7 @@ bool VisualObject::parseVisual(const JointXMLnode<>& rootNode) MRPT_TRY_END } -bool VisualObject::implParseVisual(const rapidxml::xml_node& visNode) +bool CVisualObject::implParseVisual(const rapidxml::xml_node& visNode) { MRPT_TRY_START @@ -262,7 +262,7 @@ bool VisualObject::implParseVisual(const rapidxml::xml_node& visNode) MRPT_TRY_END } -void VisualObject::showCollisionShape(bool show) +void CVisualObject::showCollisionShape(bool show) { if (!glCollision_) { @@ -271,7 +271,7 @@ void VisualObject::showCollisionShape(bool show) glCollision_->setVisibility(show); } -void VisualObject::customVisualVisible(const bool visible) +void CVisualObject::customVisualVisible(const bool visible) { if (!glCustomVisual_) { @@ -280,13 +280,13 @@ void VisualObject::customVisualVisible(const bool visible) glCustomVisual_->setVisibility(visible); } -bool VisualObject::customVisualVisible() const +bool CVisualObject::customVisualVisible() const { return glCustomVisual_ && glCustomVisual_->isVisible(); } -void VisualObject::addCustomVisualization( - const mrpt::opengl::CRenderizable::Ptr& glModel, const mrpt::poses::CPose3D& modelPose, +void CVisualObject::addCustomVisualization( + const mrpt::viz::CVisualObject::Ptr& glModel, const mrpt::poses::CPose3D& modelPose, const float modelScale, const std::string& modelName, const std::optional& modelURI, const bool initialShowBoundingBox, const std::optional& scaleOverride) @@ -319,7 +319,7 @@ void VisualObject::addCustomVisualization( // Calculate its convex hull: const auto shape = chc.get(*glModel, zMin, zMax, modelPose, effectiveScale, modelURI); - auto glGroup = mrpt::opengl::CSetOfObjects::Create(); + auto glGroup = mrpt::viz::CSetOfObjects::Create(); // Note: we cannot apply pose/scale to the original glModel since // it may be shared (many instances of the same object): @@ -331,7 +331,7 @@ void VisualObject::addCustomVisualization( if (!glCustomVisual_) { - glCustomVisual_ = mrpt::opengl::CSetOfObjects::Create(); + glCustomVisual_ = mrpt::viz::CSetOfObjects::Create(); glCustomVisual_->setName("glCustomVisual"); } glCustomVisual_->insert(glGroup); diff --git a/modules/simulator/src/Wheel.cpp b/modules/simulator/src/Wheel.cpp index d6cabd5d..86226848 100644 --- a/modules/simulator/src/Wheel.cpp +++ b/modules/simulator/src/Wheel.cpp @@ -8,10 +8,10 @@ +-------------------------------------------------------------------------+ */ #include -#include -#include -#include #include +#include +#include +#include #include #include @@ -22,9 +22,9 @@ using namespace mvsim; using namespace std; -Wheel::Wheel(World* world) : VisualObject(world) { recalcInertia(); } +Wheel::Wheel(World* world) : CVisualObject(world) { recalcInertia(); } -void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene) +void Wheel::getAs3DObject(mrpt::viz::CSetOfObjects& obj, bool isPhysicalScene) { obj.clear(); @@ -35,18 +35,18 @@ void Wheel::getAs3DObject(mrpt::opengl::CSetOfObjects& obj, bool isPhysicalScene else { auto gl_wheel = - mrpt::opengl::CCylinder::Create(0.5 * diameter, 0.5 * diameter, this->width, 15); + mrpt::viz::CCylinder::Create(0.5 * diameter, 0.5 * diameter, this->width, 15); gl_wheel->setColor_u8(color); gl_wheel->setPose(mrpt::poses::CPose3D(0, 0.5 * width, 0, 0, 0, mrpt::DEG2RAD(90))); if (!isPhysicalScene) { - auto gl_wheel_frame = mrpt::opengl::CSetOfObjects::Create(); + auto gl_wheel_frame = mrpt::viz::CSetOfObjects::Create(); gl_wheel_frame->setName("gl_wheel_frame"); gl_wheel_frame->insert(gl_wheel); { - mrpt::opengl::CSetOfObjects::Ptr gl_xyz = - mrpt::opengl::stock_objects::CornerXYZSimple(0.9 * diameter, 2.0); + mrpt::viz::CSetOfObjects::Ptr gl_xyz = + mrpt::viz::stock_objects::CornerXYZSimple(0.9 * diameter, 2.0); gl_xyz->castShadows(false); gl_wheel_frame->insert(gl_xyz); } @@ -97,8 +97,8 @@ void Wheel::recalcInertia() } void Wheel::internalGuiUpdate( - [[maybe_unused]] const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + [[maybe_unused]] const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { // nothing to do, already done in getAs3DObject() diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index e84f62e8..278c9dee 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -75,7 +75,12 @@ void World::internal_initialize() ASSERT_(!initialized_); ASSERT_(worldVisual_); - worldVisual_->getViewport()->lightParameters().ambient = lightOptions_.light_ambient; + { + auto& vlp = worldVisual_->getViewport()->lightParameters(); + vlp.ambient = lightOptions_.light_ambient; + vlp.ambientSkyColor = mrpt::img::TColorf(lightOptions_.ambient_sky_color); + vlp.ambientGroundColor = mrpt::img::TColorf(lightOptions_.ambient_ground_color); + } // Physical world light = visual world lights: worldPhysical_.getViewport()->lightParameters() = @@ -83,7 +88,7 @@ void World::internal_initialize() // Create group for sensor viz: { - auto glVizSensors = mrpt::opengl::CSetOfObjects::Create(); + auto glVizSensors = mrpt::viz::CSetOfObjects::Create(); glVizSensors->setName("group_sensors_viz"); glVizSensors->setVisibility(guiOptions_.show_sensor_points); worldVisual_->insert(glVizSensors); @@ -205,7 +210,7 @@ void World::free_opengl_resources() worldPhysical_.clear(); worldVisual_->clear(); - VisualObject::FreeOpenGLResources(); + CVisualObject::FreeOpenGLResources(); } bool World::sensor_has_to_create_egl_context() diff --git a/modules/simulator/src/WorldElements/ElevationMap.cpp b/modules/simulator/src/WorldElements/ElevationMap.cpp index f59d8e12..7a7de3ba 100644 --- a/modules/simulator/src/WorldElements/ElevationMap.cpp +++ b/modules/simulator/src/WorldElements/ElevationMap.cpp @@ -8,8 +8,8 @@ +-------------------------------------------------------------------------+ */ #include -#include #include +#include #include #include #include @@ -138,7 +138,8 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) sElevationImgFile = world_->local_to_abs_path(sElevationImgFile); mrpt::img::CImage imgElev; - if (!imgElev.loadFromFile(sElevationImgFile, 0 /*force load grayscale*/)) + if (!imgElev.loadFromFile( + sElevationImgFile, mrpt::img::TImageChannels::CH_GRAY /*force load grayscale*/)) { throw std::runtime_error(mrpt::format( "[ElevationMap] ERROR: Cannot read elevation image '%s'", @@ -272,8 +273,8 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) { mrpt::img::CImage im; mesh_image->rotateImage( - im, mrpt::DEG2RAD(texture_rotate), mesh_image->getWidth() / 2, - mesh_image->getHeight() / 2); + im, mrpt::DEG2RAD(texture_rotate), + {mesh_image->getWidth() / 2, mesh_image->getHeight() / 2}); mesh_image = std::move(im); } break; @@ -341,7 +342,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) if (model_split_size_ == 0) { // One single mesh: - auto gl_mesh = mrpt::opengl::CMesh::Create(); + auto gl_mesh = mrpt::viz::CMesh::Create(); gl_meshes_.push_back(gl_mesh); gl_mesh->enableTransparency(false); @@ -390,7 +391,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) const auto subEle = elevation_data.extractMatrix(lenIx, lenIy, startIx, startIy); // One sub-mesh: - auto gl_mesh = mrpt::opengl::CMesh::Create(); + auto gl_mesh = mrpt::viz::CMesh::Create(); gl_meshes_.push_back(gl_mesh); gl_mesh->enableTransparency(false); @@ -422,9 +423,8 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node* root) } void ElevationMap::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { ASSERTMSG_( !gl_meshes_.empty(), diff --git a/modules/simulator/src/WorldElements/GroundGrid.cpp b/modules/simulator/src/WorldElements/GroundGrid.cpp index bde4bde2..248f59d7 100644 --- a/modules/simulator/src/WorldElements/GroundGrid.cpp +++ b/modules/simulator/src/WorldElements/GroundGrid.cpp @@ -7,7 +7,7 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include +#include #include #include @@ -31,7 +31,7 @@ GroundGrid::GroundGrid(World* parent, const rapidxml::xml_node* root) line_width_(1.0) { // Create opengl object: in this class, we'll store most state data directly - // in the mrpt::opengl object. + // in the mrpt::viz object. loadConfigFrom(root); } @@ -61,8 +61,8 @@ void GroundGrid::loadConfigFrom(const rapidxml::xml_node* root) } void GroundGrid::internalGuiUpdate( - const mrpt::optional_ref& viz, - [[maybe_unused]] const mrpt::optional_ref& physical, + const mrpt::optional_ref& viz, + [[maybe_unused]] const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; @@ -70,7 +70,7 @@ void GroundGrid::internalGuiUpdate( // 1st call OR gridmap changed? if (!gl_groundgrid_ && viz) { - gl_groundgrid_ = mrpt::opengl::CGridPlaneXY::Create(); + gl_groundgrid_ = mrpt::viz::CGridPlaneXY::Create(); gl_groundgrid_->setPlaneLimits(x_min_, x_max_, y_min_, y_max_); gl_groundgrid_->setGridFrequency(interval_); gl_groundgrid_->setColor_u8(color_); diff --git a/modules/simulator/src/WorldElements/HorizontalPlane.cpp b/modules/simulator/src/WorldElements/HorizontalPlane.cpp index e54226b9..dc8180ad 100644 --- a/modules/simulator/src/WorldElements/HorizontalPlane.cpp +++ b/modules/simulator/src/WorldElements/HorizontalPlane.cpp @@ -7,9 +7,9 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include #include #include +#include #include #include @@ -26,12 +26,10 @@ HorizontalPlane::HorizontalPlane(World* parent, const rapidxml::xml_node* : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly - // in the mrpt::opengl object. + // in the mrpt::viz object. HorizontalPlane::loadConfigFrom(root); } -HorizontalPlane::~HorizontalPlane() {} - void HorizontalPlane::loadConfigFrom(const rapidxml::xml_node* root) { if (!root) return; // Assume defaults @@ -66,19 +64,18 @@ void HorizontalPlane::loadConfigFrom(const rapidxml::xml_node* root) } void HorizontalPlane::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; using namespace std::string_literals; - if (!glGroup_) glGroup_ = mrpt::opengl::CSetOfObjects::Create(); + if (!glGroup_) glGroup_ = mrpt::viz::CSetOfObjects::Create(); // 1st call? (w/o texture) if (!gl_plane_ && textureFileName_.empty() && viz && physical) { - gl_plane_ = mrpt::opengl::CTexturedPlane::Create(); + gl_plane_ = mrpt::viz::CTexturedPlane::Create(); gl_plane_->setPlaneCorners(x_min_, x_max_, y_min_, y_max_); gl_plane_->setLocation(0, 0, z_); gl_plane_->setName("HorizontalPlane_"s + getName()); @@ -86,7 +83,7 @@ void HorizontalPlane::internalGuiUpdate( gl_plane_->enableLighting(enableShadows_); gl_plane_->setColor_u8(color_); gl_plane_->cullFaces( - mrpt::typemeta::TEnumType::name2value(cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); glGroup_->insert(gl_plane_); viz->get().insert(glGroup_); physical->get().insert(glGroup_); @@ -107,13 +104,13 @@ void HorizontalPlane::internalGuiUpdate( float u_max = (x_max_ - x_min_) / textureSizeX_; float v_max = (y_max_ - y_min_) / textureSizeY_; - gl_plane_text_ = mrpt::opengl::CSetOfTexturedTriangles::Create(); + gl_plane_text_ = mrpt::viz::CSetOfTexturedTriangles::Create(); gl_plane_text_->setName("HorizontalPlane_"s + getName()); gl_plane_text_->enableLight(enableShadows_); { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = {x_min_, y_min_, z_}; t.vertices[1].xyzrgba.pt = {x_max_, y_min_, z_}; t.vertices[2].xyzrgba.pt = {x_max_, y_max_, z_}; @@ -122,11 +119,14 @@ void HorizontalPlane::internalGuiUpdate( t.vertices[1].uv = {u_max, v_min}; t.vertices[2].uv = {u_max, v_max}; + // Set vertex colors to white so texture is not darkened: + t.setColor(mrpt::img::TColor(0xff, 0xff, 0xff, 0xff)); + t.computeNormals(); gl_plane_text_->insertTriangle(t); } { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = {x_min_, y_min_, z_}; t.vertices[1].xyzrgba.pt = {x_max_, y_max_, z_}; t.vertices[2].xyzrgba.pt = {x_min_, y_max_, z_}; @@ -135,6 +135,9 @@ void HorizontalPlane::internalGuiUpdate( t.vertices[1].uv = {u_max, v_max}; t.vertices[2].uv = {u_min, v_max}; + // Set vertex colors to white so texture is not darkened: + t.setColor(mrpt::img::TColor(0xff, 0xff, 0xff, 0xff)); + t.computeNormals(); gl_plane_text_->insertTriangle(t); } @@ -142,7 +145,7 @@ void HorizontalPlane::internalGuiUpdate( gl_plane_text_->assignImage(texture); gl_plane_text_->cullFaces( - mrpt::typemeta::TEnumType::name2value(cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); glGroup_->insert(gl_plane_text_); viz->get().insert(glGroup_); diff --git a/modules/simulator/src/WorldElements/OccupancyGridMap.cpp b/modules/simulator/src/WorldElements/OccupancyGridMap.cpp index 170dfda1..8510b824 100644 --- a/modules/simulator/src/WorldElements/OccupancyGridMap.cpp +++ b/modules/simulator/src/WorldElements/OccupancyGridMap.cpp @@ -9,11 +9,11 @@ #include #include -#include #include #include #include #include +#include #include #include @@ -95,16 +95,15 @@ void OccupancyGridMap::doLoadConfigFrom(const rapidxml::xml_node* root) } void OccupancyGridMap::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; // 1st time call?? -> Create objects if (!gl_grid_ && viz && physical) { - gl_grid_ = mrpt::opengl::CSetOfObjects::Create(); + gl_grid_ = mrpt::viz::CSetOfObjects::Create(); gl_grid_->setName("OccupancyGridMap"); viz->get().insert(gl_grid_); physical->get().insert(gl_grid_); @@ -127,10 +126,10 @@ void OccupancyGridMap::internalGuiUpdate( std::lock_guard csl(gl_obs_clouds_buffer_cs_); for (size_t i = 0; i < gl_obs_clouds_.size(); i++) { - mrpt::opengl::CSetOfObjects::Ptr& gl_objs = gl_obs_clouds_[i]; + mrpt::viz::CSetOfObjects::Ptr& gl_objs = gl_obs_clouds_[i]; if (!gl_objs) { - gl_objs = mrpt::opengl::CSetOfObjects::Create(); + gl_objs = mrpt::viz::CSetOfObjects::Create(); gl_objs->setName( "OccupancyGridMap"s + this->getName() + ".obstacles["s + std::to_string(i) + "]"s); @@ -232,10 +231,10 @@ void OccupancyGridMap::simul_pre_timestep([[maybe_unused]] const TSimulContext& // GL: // 1st usage? - mrpt::opengl::CPointCloud::Ptr& gl_pts = gl_obs_clouds_buffer_[obj_idx]; + mrpt::viz::CPointCloud::Ptr& gl_pts = gl_obs_clouds_buffer_[obj_idx]; if (show_grid_collision_points_) { - gl_pts = mrpt::opengl::CPointCloud::Create(); + gl_pts = mrpt::viz::CPointCloud::Create(); gl_pts->setPointSize(4.0f); gl_pts->setColor(0, 0, 1); @@ -303,7 +302,7 @@ void OccupancyGridMap::simul_pre_timestep([[maybe_unused]] const TSimulContext& if (gl_pts && show_grid_collision_points_) { - gl_pts->mrpt::opengl::CPointCloud::insertPoint(lx, ly, .0); + gl_pts->mrpt::viz::CPointCloud::insertPoint(lx, ly, .0); } } } diff --git a/modules/simulator/src/WorldElements/PointCloud.cpp b/modules/simulator/src/WorldElements/PointCloud.cpp index 9b05388e..f58ce41e 100644 --- a/modules/simulator/src/WorldElements/PointCloud.cpp +++ b/modules/simulator/src/WorldElements/PointCloud.cpp @@ -8,7 +8,7 @@ +-------------------------------------------------------------------------+ */ #include -#include +#include #include #include @@ -65,16 +65,15 @@ void PointCloud::doLoadConfigFrom(const rapidxml::xml_node* root) } void PointCloud::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; // 1st time call?? -> Create objects if (!gl_points_ && viz && physical) { - gl_points_ = mrpt::opengl::CSetOfObjects::Create(); + gl_points_ = mrpt::viz::CSetOfObjects::Create(); gl_points_->setName("PointCloud"); gl_points_->setPose(parent()->applyWorldRenderOffset(pointcloud_pose_)); diff --git a/modules/simulator/src/WorldElements/SkyBox.cpp b/modules/simulator/src/WorldElements/SkyBox.cpp index 94b119df..fdb21958 100644 --- a/modules/simulator/src/WorldElements/SkyBox.cpp +++ b/modules/simulator/src/WorldElements/SkyBox.cpp @@ -11,8 +11,8 @@ #include // -#include #include +#include #include @@ -25,7 +25,7 @@ using namespace std; SkyBox::SkyBox(World* parent, const rapidxml::xml_node* root) : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly - // in the mrpt::opengl object. + // in the mrpt::viz object. loadConfigFrom(root); } @@ -53,7 +53,7 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) "Texture pattern URI in ... must contain one '%s' " "placeholder."); - using mrpt::opengl::CUBE_TEXTURE_FACE; + using mrpt::viz::CUBE_TEXTURE_FACE; const std::vector> faceImages = { {CUBE_TEXTURE_FACE::FRONT, "Front"}, {CUBE_TEXTURE_FACE::BACK, "Back"}, @@ -61,7 +61,7 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) {CUBE_TEXTURE_FACE::LEFT, "Left"}, {CUBE_TEXTURE_FACE::RIGHT, "Right"}, }; - auto sb = mrpt::opengl::CSkyBox::Create(); + auto sb = mrpt::viz::CSkyBox::Create(); for (const auto& p : faceImages) { @@ -77,9 +77,8 @@ void SkyBox::loadConfigFrom(const rapidxml::xml_node* root) } void SkyBox::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; diff --git a/modules/simulator/src/WorldElements/VerticalPlane.cpp b/modules/simulator/src/WorldElements/VerticalPlane.cpp index 03574cb6..58c6a129 100644 --- a/modules/simulator/src/WorldElements/VerticalPlane.cpp +++ b/modules/simulator/src/WorldElements/VerticalPlane.cpp @@ -7,9 +7,9 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include #include #include +#include #include #include @@ -27,7 +27,7 @@ VerticalPlane::VerticalPlane(World* parent, const rapidxml::xml_node* root : WorldElementBase(parent) { // Create opengl object: in this class, we'll store most state data directly - // in the mrpt::opengl object. + // in the mrpt::viz object. VerticalPlane::loadConfigFrom(root); } @@ -304,14 +304,13 @@ void VerticalPlane::createPhysicsBodies() } void VerticalPlane::internalGuiUpdate( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical, - [[maybe_unused]] bool childrenOnly) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical, [[maybe_unused]] bool childrenOnly) { using namespace mrpt::math; using namespace std::string_literals; - if (!glGroup_) glGroup_ = mrpt::opengl::CSetOfObjects::Create(); + if (!glGroup_) glGroup_ = mrpt::viz::CSetOfObjects::Create(); // Create visual representation on first call if (!gl_plane_ && !gl_plane_text_ && viz && physical) @@ -330,8 +329,8 @@ void VerticalPlane::internalGuiUpdate( } void VerticalPlane::createVisualRepresentation( - const mrpt::optional_ref& viz, - const mrpt::optional_ref& physical) + const mrpt::optional_ref& viz, + const mrpt::optional_ref& physical) { using namespace mrpt::math; using namespace std::string_literals; @@ -361,7 +360,7 @@ void VerticalPlane::createVisualRepresentation( ASSERT_(has_texture); } - gl_plane_text_ = mrpt::opengl::CSetOfTexturedTriangles::Create(); + gl_plane_text_ = mrpt::viz::CSetOfTexturedTriangles::Create(); gl_plane_text_->setName("VerticalPlane_"s + getName()); gl_plane_text_->enableLight(enableShadows_); @@ -385,7 +384,7 @@ void VerticalPlane::createVisualRepresentation( } gl_plane_text_->cullFaces( - mrpt::typemeta::TEnumType::name2value(cull_faces_)); + mrpt::typemeta::TEnumType::name2value(cull_faces_)); glGroup_->insert(gl_plane_text_); viz->get().insert(glGroup_); @@ -495,7 +494,7 @@ void VerticalPlane::renderWallFace( // Triangle 1 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; if (is_front_face) { t.vertices[0].xyzrgba.pt = {seg_p0_2d.x, seg_p0_2d.y, seg.z_start}; @@ -525,7 +524,7 @@ void VerticalPlane::renderWallFace( // Triangle 2 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; if (is_front_face) { t.vertices[0].xyzrgba.pt = {seg_p0_2d.x, seg_p0_2d.y, seg.z_start}; @@ -558,7 +557,7 @@ void VerticalPlane::renderWallFace( // Plain color // Triangle 1 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; if (is_front_face) { t.vertices[0].xyzrgba.pt = {seg_p0_2d.x, seg_p0_2d.y, seg.z_start}; @@ -582,7 +581,7 @@ void VerticalPlane::renderWallFace( // Triangle 2 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; if (is_front_face) { t.vertices[0].xyzrgba.pt = {seg_p0_2d.x, seg_p0_2d.y, seg.z_start}; @@ -630,7 +629,7 @@ void VerticalPlane::renderWallEdges( { // Triangle 1 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = v0; t.vertices[1].xyzrgba.pt = v1; t.vertices[2].xyzrgba.pt = v2; @@ -654,7 +653,7 @@ void VerticalPlane::renderWallEdges( // Triangle 2 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = v0; t.vertices[1].xyzrgba.pt = v2; t.vertices[2].xyzrgba.pt = v3; @@ -716,7 +715,7 @@ void VerticalPlane::renderOpeningReveals( { // Triangle 1 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = v0; t.vertices[1].xyzrgba.pt = v1; t.vertices[2].xyzrgba.pt = v2; @@ -740,7 +739,7 @@ void VerticalPlane::renderOpeningReveals( // Triangle 2 { - mrpt::opengl::CSetOfTexturedTriangles::TTriangle t; + mrpt::viz::CSetOfTexturedTriangles::TTriangle t; t.vertices[0].xyzrgba.pt = v0; t.vertices[1].xyzrgba.pt = v2; t.vertices[2].xyzrgba.pt = v3; diff --git a/modules/simulator/src/World_gui.cpp b/modules/simulator/src/World_gui.cpp index 694cb32a..31038d8f 100644 --- a/modules/simulator/src/World_gui.cpp +++ b/modules/simulator/src/World_gui.cpp @@ -16,9 +16,9 @@ #include #include #include -#include #include #include +#include #include #include // cos(), sin() @@ -35,10 +35,106 @@ void World::TGUI_Options::parse_from( parse_xmlnode_children_as_param(node, params, {}, "[World::TGUI_Options]", &logger); } +// Helper: read an XML child's text as float, return default if missing. +static float xmlChildFloat(const rapidxml::xml_node& parent, const char* name, float def) +{ + auto* n = parent.first_node(name); + return n ? std::stof(std::string(n->value(), n->value_size())) : def; +} + +// Helper: read an XML child's text as "x y z" into TPoint3Df/TVector3Df. +static mrpt::math::TPoint3Df xmlChildPoint3f( + const rapidxml::xml_node& parent, const char* name, const mrpt::math::TPoint3Df& def) +{ + auto* n = parent.first_node(name); + if (!n) return def; + float x = 0, y = 0, z = 0; + if (3 != std::sscanf(n->value(), "%f %f %f", &x, &y, &z)) + throw std::runtime_error( + mrpt::format("[World::LightOptions] Error parsing '<%s>': expected 'X Y Z'", name)); + return {x, y, z}; +} + +// Helper: read an XML child's text as #RRGGBB[AA] into TColorf. +static mrpt::img::TColorf xmlChildColorf( + const rapidxml::xml_node& parent, const char* name, const mrpt::img::TColorf& def) +{ + auto* n = parent.first_node(name); + if (!n) return def; + std::string str(n->value(), n->value_size()); + if (str.size() < 2 || str[0] != '#') + throw std::runtime_error(mrpt::format( + "[World::LightOptions] Error parsing '<%s>': expected " + "'#RRGGBB[AA]'", + name)); + unsigned int r, g, b, a = 0xff; + int ret = std::sscanf(str.c_str() + 1, "%2x%2x%2x%2x", &r, &g, &b, &a); + if (ret != 3 && ret != 4) + throw std::runtime_error(mrpt::format( + "[World::LightOptions] Error parsing '<%s>': expected " + "'#RRGGBB[AA]'", + name)); + return mrpt::img::TColorf(mrpt::img::TColor(r, g, b, a)); +} + void World::LightOptions::parse_from( const rapidxml::xml_node& node, mrpt::system::COutputLogger& logger) { - parse_xmlnode_children_as_param(node, params, {}, "[World::LightOptions]", &logger); + // Parse scalar parameters, skipping point_light/spot_light child nodes: + for (auto* child = node.first_node(); child; child = child->next_sibling(nullptr)) + { + const std::string name(child->name(), child->name_size()); + if (name == "point_light" || name == "spot_light") continue; + + if (!parse_xmlnode_as_param(*child, params, {}, "[World::LightOptions]")) + { + logger.logFmt( + mrpt::system::LVL_WARN, "Unrecognized tag '<%s>' in [World::LightOptions]", + name.c_str()); + } + } + + // Parse children: + for (auto* n = node.first_node("point_light"); n; n = n->next_sibling("point_light")) + { + const auto pos = xmlChildPoint3f(*n, "position", {0, 0, 3}); + const auto color = xmlChildColorf(*n, "color", {1.0f, 1.0f, 1.0f}); + const float diffuse = xmlChildFloat(*n, "diffuse", 0.8f); + const float specular = xmlChildFloat(*n, "specular", 0.5f); + const float att_const = xmlChildFloat(*n, "attenuation_constant", 1.0f); + const float att_lin = xmlChildFloat(*n, "attenuation_linear", 0.09f); + const float att_quad = xmlChildFloat(*n, "attenuation_quadratic", 0.032f); + + extra_lights.push_back(mrpt::viz::TLight::PointLight( + pos, color, diffuse, specular, att_const, att_lin, att_quad)); + + logger.logFmt( + mrpt::system::LVL_INFO, "[LightOptions] Parsed point_light at (%.1f, %.1f, %.1f)", + pos.x, pos.y, pos.z); + } + + // Parse children: + for (auto* n = node.first_node("spot_light"); n; n = n->next_sibling("spot_light")) + { + const auto pos = xmlChildPoint3f(*n, "position", {0, 0, 3}); + const auto dir = xmlChildPoint3f(*n, "direction", {0, 0, -1}); + const auto color = xmlChildColorf(*n, "color", {1.0f, 1.0f, 1.0f}); + const float diffuse = xmlChildFloat(*n, "diffuse", 0.8f); + const float specular = xmlChildFloat(*n, "specular", 0.5f); + const float inner_deg = xmlChildFloat(*n, "inner_cutoff_deg", 12.5f); + const float outer_deg = xmlChildFloat(*n, "outer_cutoff_deg", 17.5f); + const float att_const = xmlChildFloat(*n, "attenuation_constant", 1.0f); + const float att_lin = xmlChildFloat(*n, "attenuation_linear", 0.09f); + const float att_quad = xmlChildFloat(*n, "attenuation_quadratic", 0.032f); + + extra_lights.push_back(mrpt::viz::TLight::SpotLight( + pos, dir, inner_deg, outer_deg, color, diffuse, specular, att_const, att_lin, + att_quad)); + + logger.logFmt( + mrpt::system::LVL_INFO, "[LightOptions] Parsed spot_light at (%.1f, %.1f, %.1f)", pos.x, + pos.y, pos.z); + } } //!< Return true if the GUI window is open, after a previous call to @@ -87,7 +183,7 @@ void World::GUI::prepare_control_window() }); w->add( - "Orthogonal view", [&](bool b) { gui_win->camera().setCameraProjective(!b); }) + "Orthogonal view", [&](bool b) { gui_win->camera().setProjectiveModel(!b); }) ->setChecked(parent_.guiOptions_.ortho); w->add( @@ -138,7 +234,7 @@ void World::GUI::prepare_control_window() { std::lock_guard lck(gui_win->background_scene_mtx); - auto glVizSensors = std::dynamic_pointer_cast( + auto glVizSensors = std::dynamic_pointer_cast( gui_win->background_scene->getByName("group_sensors_viz")); ASSERT_(glVizSensors); @@ -171,7 +267,7 @@ void World::GUI::prepare_control_window() auto lck = mrpt::lockHelper(parent_.simulableObjectsMtx_); for (auto& s : parent_.simulableObjects_) { - auto* vis = dynamic_cast(s.second.get()); + auto* vis = dynamic_cast(s.second.get()); if (!vis) continue; vis->showCollisionShape(b); } @@ -283,24 +379,24 @@ void World::GUI::prepare_editor_window() if (auto v = dynamic_cast(o.second.get()); v) { wrapperIdx = 0; - ipo.visual = dynamic_cast(v); + ipo.visual = dynamic_cast(v); } if (auto v = dynamic_cast(o.second.get()); v) { wrapperIdx = 2; - ipo.visual = dynamic_cast(v); + ipo.visual = dynamic_cast(v); } if (auto v = dynamic_cast(o.second.get()); v) { wrapperIdx = 1; - ipo.visual = dynamic_cast(v); + ipo.visual = dynamic_cast(v); } // bool isWorldElement = false; if (auto v = dynamic_cast(o.second.get()); v) { // isWorldElement = true; wrapperIdx = 3; - ipo.visual = dynamic_cast(v); + ipo.visual = dynamic_cast(v); } if (wrapperIdx < 0) @@ -639,11 +735,11 @@ void World::internal_GUI_thread() gui_.gui_win->performLayout(); auto& cam = gui_.gui_win->camera(); - cam.setCameraProjective(!guiOptions_.ortho); + cam.setProjectiveModel(!guiOptions_.ortho); cam.setZoomDistance(guiOptions_.camera_distance); cam.setAzimuthDegrees(guiOptions_.camera_azimuth_deg); cam.setElevationDegrees(guiOptions_.camera_elevation_deg); - cam.setCameraFOV(guiOptions_.fov_deg); + cam.setFOVdeg(guiOptions_.fov_deg); const auto p = this->worldRenderOffset() + guiOptions_.camera_point_to; cam.setCameraPointing(p.x, p.y, p.z); @@ -655,18 +751,31 @@ void World::internal_GUI_thread() auto vv = worldVisual_->getViewport(); auto vp = worldPhysical_.getViewport(); - auto lambdaSetLightParams = [&lo](const mrpt::opengl::COpenGLViewport::Ptr& v) + auto lambdaSetLightParams = [&lo](const mrpt::viz::Viewport::Ptr& v) { // enable shadows and set the shadow map texture size: const int sms = lo.shadow_map_size; v->enableShadowCasting(lo.enable_shadows, sms, sms); - // light color: + // light color and intensities: const auto colf = mrpt::img::TColorf(lo.light_color); auto& vlp = v->lightParameters(); - vlp.color = colf; + if (!vlp.lights.empty()) + { + vlp.lights[0].color = colf; + vlp.lights[0].diffuse = lo.light_diffuse; + vlp.lights[0].specular = lo.light_specular; + } + + // Hemisphere ambient lighting (replaces fill light): + vlp.ambient = lo.light_ambient; + vlp.ambientSkyColor = mrpt::img::TColorf(lo.ambient_sky_color); + vlp.ambientGroundColor = mrpt::img::TColorf(lo.ambient_ground_color); + + // Add extra lights (point and spot) from XML: + for (const auto& el : lo.extra_lights) vlp.lights.push_back(el); vlp.eyeDistance2lightShadowExtension = lo.eye_distance_to_shadow_map_extension; @@ -779,7 +888,8 @@ void World::internal_GUI_thread() auto lck = mrpt::lockHelper(gui_.gui_win->background_scene_mtx); if (gui_.gui_win->background_scene) { - gui_.gui_win->background_scene->freeOpenGLResources(); + // In mrpt3, OpenGL resources are freed automatically + gui_.gui_win->background_scene.reset(); } } @@ -792,7 +902,7 @@ void World::internal_GUI_thread() lckListObjs.unlock(); - VisualObject::FreeOpenGLResources(); + // CVisualObject::FreeOpenGLResources() removed in mrpt3 (automatic) // Now, destroy window: gui_.gui_win.reset(); @@ -813,7 +923,7 @@ void World::GUI::handle_mouse_operations() { return; } - mrpt::opengl::COpenGLViewport::Ptr vp; + mrpt::viz::Viewport::Ptr vp; { auto lck = mrpt::lockHelper(gui_win->background_scene_mtx); if (!gui_win->background_scene) @@ -826,7 +936,10 @@ void World::GUI::handle_mouse_operations() const auto mousePt = gui_win->mousePos(); mrpt::math::TLine3D ray; - vp->get3DRayForPixelCoord(mousePt.x(), mousePt.y(), ray); + auto rayOpt = + vp->get3DRayForPixelCoord({static_cast(mousePt.x()), static_cast(mousePt.y())}); + if (!rayOpt.has_value()) return; + ray = rayOpt.value(); // Create a 3D plane, i.e. Z=0 const auto ground_plane = mrpt::math::TPlane::From3Points({0, 0, 0}, {1, 0, 0}, {0, 1, 0}); @@ -887,18 +1000,18 @@ void World::GUI::handle_mouse_operations() void World::internal_process_pending_gui_user_tasks() { - guiUserPendingTasksMtx_.lock(); - - for (const auto& task : guiUserPendingTasks_) + std::vector> tasks; { - task(); + std::lock_guard lck(guiUserPendingTasksMtx_); + tasks = std::move(guiUserPendingTasks_); + guiUserPendingTasks_.clear(); } - guiUserPendingTasks_.clear(); - guiUserPendingTasksMtx_.unlock(); + // Execute tasks outside the mutex to avoid holding it during callbacks: + for (const auto& task : tasks) task(); } -void World::internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene& physicalObjects) +void World::internalRunSensorsOn3DScene(mrpt::viz::Scene& physicalObjects) { auto tle = mrpt::system::CTimeLoggerEntry(timlogger_, "internalRunSensorsOn3DScene"); @@ -910,8 +1023,7 @@ void World::internalRunSensorsOn3DScene(mrpt::opengl::COpenGLScene& physicalObje clear_pending_running_sensors_on_3D_scene(); } -void World::internalUpdate3DSceneObjects( - mrpt::opengl::COpenGLScene& viz, mrpt::opengl::COpenGLScene& physical) +void World::internalUpdate3DSceneObjects(mrpt::viz::Scene& viz, mrpt::viz::Scene& physical) { // Update view of map elements // ----------------------------- @@ -953,10 +1065,10 @@ void World::internalUpdate3DSceneObjects( { static const std::string kJointsGlName = "__joint_lines"; auto glJoints = - std::dynamic_pointer_cast(viz.getByName(kJointsGlName)); + std::dynamic_pointer_cast(viz.getByName(kJointsGlName)); if (!glJoints) { - glJoints = mrpt::opengl::CSetOfLines::Create(); + glJoints = mrpt::viz::CSetOfLines::Create(); glJoints->setName(kJointsGlName); glJoints->setLineWidth(2.0f); glJoints->setColor_u8(0xff, 0xcc, 0x00, 0xcc); @@ -1215,7 +1327,7 @@ mrpt::math::TPoint2D World::internal_gui_on_image( auto lck = mrpt::lockHelper(glControl->scene_mtx); - glControl->scene = mrpt::opengl::COpenGLScene::Create(); + glControl->scene = mrpt::viz::Scene::Create(); gui_.gui_win->performLayout(); } @@ -1260,6 +1372,8 @@ void World::internalGraphicsLoopTasksForSimulation() // abort. Otherwise, the error may repeat over and over forever // and the main thread will never know about it. MRPT_LOG_ERROR(e.what()); + // Clear this flag so the simulation thread's busy-wait can exit: + clear_pending_running_sensors_on_3D_scene(); simulator_must_close(true); } } @@ -1276,6 +1390,8 @@ void World::setLightDirectionFromAzimuthElevation(const float azimuth, const flo auto vv = worldVisual_->getViewport(); auto vp = worldPhysical_.getViewport(); - vv->lightParameters().direction = dir; - vp->lightParameters().direction = dir; + if (!vv->lightParameters().lights.empty()) + vv->lightParameters().lights[0].direction = dir; + if (!vp->lightParameters().lights.empty()) + vp->lightParameters().lights[0].direction = dir; } diff --git a/modules/simulator/src/World_load_xml.cpp b/modules/simulator/src/World_load_xml.cpp index 739ee71d..a6012d95 100644 --- a/modules/simulator/src/World_load_xml.cpp +++ b/modules/simulator/src/World_load_xml.cpp @@ -442,7 +442,7 @@ void World::parse_tag_marker(const XmlParserContext& ctx) ASSERTMSG_( parsedOk, "Error parsing XYZ data within : "s + parseError); - auto glObj = mrpt::opengl::CSetOfLines::Create(); + auto glObj = mrpt::viz::CSetOfLines::Create(); glObj->setColor_u8(color); glObj->setLocation(translation); diff --git a/modules/simulator/src/World_simul.cpp b/modules/simulator/src/World_simul.cpp index 79a2b18e..d416cd1f 100644 --- a/modules/simulator/src/World_simul.cpp +++ b/modules/simulator/src/World_simul.cpp @@ -167,7 +167,8 @@ void World::internal_one_timestep(double dt) { // Use a huge timeout here to avoid timing out in build farms / cloud // containers: - for (int i = 0; i < 20000 && pending_running_sensors_on_3D_scene(); i++) + for (int i = 0; + i < 20000 && pending_running_sensors_on_3D_scene() && !simulator_must_close(); i++) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } diff --git a/modules/simulator/src/World_walls.cpp b/modules/simulator/src/World_walls.cpp index 6247500c..cd1eee06 100644 --- a/modules/simulator/src/World_walls.cpp +++ b/modules/simulator/src/World_walls.cpp @@ -9,8 +9,9 @@ #include #include -#include #include +#include +#include #include #include @@ -184,10 +185,20 @@ void World::process_load_walls(const rapidxml::xml_node& node) MRPT_LOG_DEBUG_STREAM("Loading walls definition model from: " << localFileName); - auto glModel = mrpt::opengl::CAssimpModel::Create(); + auto glModel = mrpt::viz::CAssimpModel::Create(); glModel->loadScene(localFileName); - const auto& points = glModel->shaderWireframeVertexPointBuffer(); + // In mrpt3, CAssimpModel is a CSetOfObjects; collect line vertices from children: + glModel->updateBuffers(); + std::vector points; + for (const auto& child : *glModel) + { + if (!child) continue; + auto* lp = dynamic_cast(child.get()); + if (!lp) continue; + const auto& buf = lp->shaderLinesVertexPointBuffer(); + points.insert(points.end(), buf.begin(), buf.end()); + } MRPT_LOG_DEBUG_STREAM("Walls loaded from model file, " << points.size() << " segments."); // Transform them: diff --git a/mvsim-cli/CMakeLists.txt b/mvsim-cli/CMakeLists.txt index abfb722c..441f5656 100644 --- a/mvsim-cli/CMakeLists.txt +++ b/mvsim-cli/CMakeLists.txt @@ -7,6 +7,8 @@ if (TARGET mvsim::msgs) set(DEP_MVSIM_MSGS_TRG mvsim::msgs) endif() +find_package(CLI11 REQUIRED) + # === Define executable === add_executable(${PROJECT_NAME} @@ -22,7 +24,7 @@ target_link_libraries( mvsim::simulator ${DEP_MVSIM_MSGS_TRG} ${DEP_MVSIM_COMMS_TRG} - mrpt::tclap + CLI11::CLI11 ${BOX2D_LIBRARIES} # Box2D libs ${CMAKE_THREAD_LIBS_INIT} ) diff --git a/mvsim-cli/mvsim-cli-launch.cpp b/mvsim-cli/mvsim-cli-launch.cpp index 1db60a5b..4d6f1555 100644 --- a/mvsim-cli/mvsim-cli-launch.cpp +++ b/mvsim-cli/mvsim-cli-launch.cpp @@ -168,13 +168,13 @@ int launchSimulation() // check args: bool badArgs = false; - const auto& unlabeledArgs = cli->argCmd.getValue(); + const auto& unlabeledArgs = cli->argCmd; if (unlabeledArgs.size() != 2) { badArgs = true; } - if (cli->argHelp.isSet() || badArgs) + if (cli->argHelp || badArgs) { fprintf( stdout, @@ -193,7 +193,7 @@ Available options: mvsim_install_signal_handler(); const auto verbosityLevel = mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue()); + cli->argVerbosity); if (verbosityLevel <= mrpt::system::LVL_INFO) { @@ -214,12 +214,12 @@ Available options: app->world.setMinLoggingLevel(verbosityLevel); // CLI flags: - if (cli->argFullProfiler.isSet()) + if (cli->argFullProfiler) { app->world.getTimeLogger().enableKeepWholeHistory(); } - if (cli->argHeadless.isSet()) + if (cli->argHeadless) { app->world.headless(true); } @@ -246,7 +246,7 @@ Available options: // Launch GUI thread, unless we are in headless mode: app->thread_params.world = &app->world; - if (!cli->argHeadless.isSet()) + if (!cli->argHeadless) { // regular GUI: app->thGUI = std::thread(&mvsim_server_thread_update_GUI, std::ref(app->thread_params)); @@ -259,7 +259,7 @@ Available options: // Run simulation: const double tAbsInit = mrpt::Clock::nowDouble(); - const double rtFactor = cli->argRealTimeFactor.getValue(); + const double rtFactor = cli->argRealTimeFactor; bool doExit = false; while (!doExit) diff --git a/mvsim-cli/mvsim-cli-main.cpp b/mvsim-cli/mvsim-cli-main.cpp index f14674ab..db92a0d4 100644 --- a/mvsim-cli/mvsim-cli-main.cpp +++ b/mvsim-cli/mvsim-cli-main.cpp @@ -7,7 +7,6 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include #include #include #include // kbhit() @@ -45,13 +44,21 @@ int main(int argc, char** argv) { cli = std::make_unique(); - if (!cli->cmd.parse(argc, argv)) + try { - printListCommands(); - return 1; + cli->cmd.parse(argc, argv); + } + catch (const CLI::ParseError& e) + { + // Don't exit on parse errors — we handle unknown commands ourselves + if (e.get_exit_code() != 0) + { + printListCommands(); + return 1; + } } - if (cli->argVersion.isSet()) + if (cli->argVersion) { printVersion(); return 0; @@ -59,17 +66,17 @@ int main(int argc, char** argv) // Take first unlabeled argument: std::string command; - if (const auto& lst = cli->argCmd.getValue(); !lst.empty()) + if (!cli->argCmd.empty()) { - command = lst.at(0); + command = cli->argCmd.at(0); } // Look up command in table: auto itCmd = cliCommands.find(command); - if (!cli->argCmd.isSet() || itCmd == cliCommands.end()) + if (cli->argCmd.empty() || itCmd == cliCommands.end()) { - if (!cli->argHelp.isSet()) + if (!cli->argHelp) { setConsoleErrorColor(); std::cerr << "Error: missing or unknown command.\n"; diff --git a/mvsim-cli/mvsim-cli-node.cpp b/mvsim-cli/mvsim-cli-node.cpp index d43be362..eb534da4 100644 --- a/mvsim-cli/mvsim-cli-node.cpp +++ b/mvsim-cli/mvsim-cli-node.cpp @@ -24,8 +24,8 @@ const std::map cliNodeCommands = { int commandNode() { - const auto& lstCmds = cli->argCmd.getValue(); - if (cli->argHelp.isSet()) return printCommandsNode(false); + const auto& lstCmds = cli->argCmd; + if (cli->argHelp) return printCommandsNode(false); if (lstCmds.size() != 2) return printCommandsNode(true); // Take second unlabeled argument: @@ -44,7 +44,7 @@ int nodeList() mvsim::Client client; client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + cli->argVerbosity)); std::cout << "# Connecting to server...\n"; client.connect(); diff --git a/mvsim-cli/mvsim-cli-server.cpp b/mvsim-cli/mvsim-cli-server.cpp index 270fc702..b0656f2d 100644 --- a/mvsim-cli/mvsim-cli-server.cpp +++ b/mvsim-cli/mvsim-cli-server.cpp @@ -28,10 +28,10 @@ void commonLaunchServer() // Start network server: server = std::make_shared(); - if (cli->argPort.isSet()) server->listenningPort(cli->argPort.getValue()); + if (cli->cmd["--port"]->count() > 0) server->listenningPort(cli->argPort); server->setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + cli->argVerbosity)); server->start(); #endif @@ -40,7 +40,7 @@ void commonLaunchServer() int launchStandAloneServer() { #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) - if (cli->argHelp.isSet()) + if (cli->argHelp) { fprintf( stdout, diff --git a/mvsim-cli/mvsim-cli-topic.cpp b/mvsim-cli/mvsim-cli-topic.cpp index 21a62a55..cd3ee7b2 100644 --- a/mvsim-cli/mvsim-cli-topic.cpp +++ b/mvsim-cli/mvsim-cli-topic.cpp @@ -33,8 +33,8 @@ const std::map cliTopicCommands = { int commandTopic() { - const auto& lstCmds = cli->argCmd.getValue(); - if (cli->argHelp.isSet()) return printCommandsTopic(false); + const auto& lstCmds = cli->argCmd; + if (cli->argHelp) return printCommandsTopic(false); if (lstCmds.size() != 2 && lstCmds.size() != 3) return printCommandsTopic(true); // Take second unlabeled argument: @@ -55,7 +55,7 @@ int topicList() mvsim::Client client; client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + cli->argVerbosity)); std::cout << "# Connecting to server...\n"; client.connect(); @@ -67,7 +67,7 @@ int topicList() for (const auto& n : lstTopics) { - if (cli->argDetails.isSet()) + if (cli->argDetails) { std::cout << "- name: \"" << n.name << "\"\n" << " type: \"" << n.type << "\"\n" @@ -139,9 +139,9 @@ int topicEcho() mvsim::Client client; client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + cli->argVerbosity)); - const auto& lstCmds = cli->argCmd.getValue(); + const auto& lstCmds = cli->argCmd; if (lstCmds.size() != 3) return printCommandsTopic(true); const auto& topicName = lstCmds.at(2); @@ -168,9 +168,9 @@ int topicHz() mvsim::Client client; client.setMinLoggingLevel(mrpt::typemeta::TEnumType::name2value( - cli->argVerbosity.getValue())); + cli->argVerbosity)); - const auto& lstCmds = cli->argCmd.getValue(); + const auto& lstCmds = cli->argCmd; if (lstCmds.size() != 3) return printCommandsTopic(true); const auto& topicName = lstCmds.at(2); diff --git a/mvsim-cli/mvsim-cli.h b/mvsim-cli/mvsim-cli.h index 623b3c6c..7d484fa8 100644 --- a/mvsim-cli/mvsim-cli.h +++ b/mvsim-cli/mvsim-cli.h @@ -9,55 +9,59 @@ #pragma once -#include +#include #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) #include #endif #include #include +#include +#include -// We need all TCLAP objects to be initialized in order for all translation -// units, that is why we use this holder structure: struct cli_flags { - TCLAP::CmdLine cmd{"mvsim", ' ', "version", false /* no --help */}; + CLI::App cmd{"mvsim", "mvsim"}; + + std::vector argCmd; + std::string argVerbosity = "INFO"; + bool argFullProfiler = false; + bool argHeadless = false; + bool argDetails = false; + bool argVersion = false; + bool argHelp = false; +#if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) + int argPort = mvsim::MVSIM_PORTNO_MAIN_REP; +#endif + double argRealTimeFactor = 1.0; + + cli_flags() + { + cmd.set_help_flag(); // disable built-in --help/-h + cmd.allow_extras(true); - TCLAP::UnlabeledMultiArg argCmd{ - "command", "Command to run. Run 'mvsim help' to list commands.", false, "", cmd}; + cmd.add_option("commands", argCmd, "Command to run. Run 'mvsim help' to list commands."); - TCLAP::ValueArg argVerbosity{ - "v", "verbose", "Verbosity level", false, "INFO", "ERROR|WARN|INFO|DEBUG", cmd}; + cmd.add_option("-v,--verbose", argVerbosity, "Verbosity level"); - TCLAP::SwitchArg argFullProfiler{ - "", "full-profiler", - "Enable saving *all* timing data, dumping it to a file at the end of " - "the " - "program.", - cmd}; + cmd.add_flag("--full-profiler", argFullProfiler, + "Enable saving *all* timing data, dumping it to a file at the end of the program."); - TCLAP::SwitchArg argHeadless{"", "headless", "Runs the simulator without any GUI window.", cmd}; + cmd.add_flag("--headless", argHeadless, "Runs the simulator without any GUI window."); - TCLAP::SwitchArg argDetails{"", "details", "Shows details in the specified subcommand", cmd}; + cmd.add_flag("--details", argDetails, "Shows details in the specified subcommand"); - TCLAP::SwitchArg argVersion{"", "version", "Shows program version and exits", cmd}; + cmd.add_flag("--version", argVersion, "Shows program version and exits"); - TCLAP::SwitchArg argHelp{"h", "help", "Shows more detailed help for command", cmd}; + cmd.add_flag("-h,--help", argHelp, "Shows more detailed help for command"); #if defined(MVSIM_HAS_ZMQ) && defined(MVSIM_HAS_PROTOBUF) - TCLAP::ValueArg argPort{ - "p", "port", "TCP port to listen at", false, mvsim::MVSIM_PORTNO_MAIN_REP, "TCP port", cmd}; + cmd.add_option("-p,--port", argPort, "TCP port to listen at"); #endif - TCLAP::ValueArg argRealTimeFactor{ - "", - "realtime-factor", - "Realtime modification factor: <1 slower than real-time, >1 faster " - "than real-time", - false, - 1.0, - "1.0", - cmd}; + cmd.add_option("--realtime-factor", argRealTimeFactor, + "Realtime modification factor: <1 slower than real-time, >1 faster than real-time"); + } }; extern std::unique_ptr cli; diff --git a/mvsim-pid-tuner/CMakeLists.txt b/mvsim-pid-tuner/CMakeLists.txt index 5d4982fb..3aad898a 100644 --- a/mvsim-pid-tuner/CMakeLists.txt +++ b/mvsim-pid-tuner/CMakeLists.txt @@ -1,5 +1,7 @@ project(mvsim-pid-tuner) +find_package(CLI11 REQUIRED) + # === Define executable === add_executable(${PROJECT_NAME} mvsim-pid-tuner-main.cpp @@ -7,7 +9,7 @@ add_executable(${PROJECT_NAME} target_link_libraries( ${PROJECT_NAME} mvsim::simulator - mrpt::tclap + CLI11::CLI11 ${BOX2D_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ) diff --git a/mvsim-pid-tuner/mvsim-pid-tuner-main.cpp b/mvsim-pid-tuner/mvsim-pid-tuner-main.cpp index a3d02edd..bc07b710 100644 --- a/mvsim-pid-tuner/mvsim-pid-tuner-main.cpp +++ b/mvsim-pid-tuner/mvsim-pid-tuner-main.cpp @@ -9,12 +9,11 @@ // mvsim-pid-tuner: Automatic PID parameter tuning tool for MVSim vehicles. // // Given a vehicle definition XML file, this tool: -// 1. Runs an open-loop step response test (constant torque → wheel velocity) -// 2. Identifies the first-order plant model: G(s) = K / (τs + 1) +// 1. Runs open-loop step response tests (linear and rotational) +// 2. Identifies the first-order plant models: G(s) = K / (τs + 1) // 3. Proposes optimal PID parameters using IMC (Internal Model Control) tuning -// 4. Validates the proposed parameters with a closed-loop simulation +// 4. Validates the proposed parameters with closed-loop simulations -#include #include #include #include @@ -24,6 +23,7 @@ #include #include +#include #include #include #include @@ -35,14 +35,14 @@ struct TimeVelSample { double time = 0; - double velocity_x = 0; // wheel ground-contact longitudinal velocity (m/s) + double value = 0; // velocity (m/s) or angular velocity (rad/s) }; struct PlantModel { - double K = 0; // steady-state gain: (m/s) per (Nm of torque) + double K = 0; // steady-state gain double tau = 0; // time constant (s) - double v_ss = 0; // steady-state velocity (m/s) + double v_ss = 0; // steady-state value double torque_applied = 0; // torque used for identification }; @@ -55,37 +55,132 @@ struct PIDParams }; // ======================================================== -// Run open-loop step response with "raw" controller +// Helper: create world + vehicle // ======================================================== -static PlantModel run_open_loop_step( - const std::string& vehicle_xml_path, const std::string& vehicle_class, double test_torque, - double sim_duration, double sim_step) +static std::pair, std::shared_ptr> + create_world_and_vehicle( + const std::string& vehicle_xml_path, const std::string& vehicle_class, double sim_step) { - // Load vehicle definition - mvsim::World world; - world.headless(true); - world.set_gravity(9.81); - world.set_simul_timestep(sim_step); + auto world = std::make_shared(); + world->headless(true); + world->set_gravity(9.81); + world->set_simul_timestep(sim_step); - // Load the vehicle:class definition by wrapping it in a minimal world XML { - // default_sensors="false" to avoid requiring sensor simulation in this tool. std::string world_xml = "" " " ""; - world.load_from_XML(world_xml, vehicle_xml_path); + world->load_from_XML(world_xml, vehicle_xml_path); } - // Instantiate the vehicle from its class, then swap controller to "raw" std::string veh_xml = "" " 0 0 0" ""; - auto veh = mvsim::VehicleBase::factory(&world, veh_xml); + auto veh = mvsim::VehicleBase::factory(world.get(), veh_xml); + return {world, veh}; +} + +// ======================================================== +// Helper: compute vehicle params +// ======================================================== +struct VehicleParams +{ + double total_mass = 0; + double partial_mass = 0; + double R = 0; // wheel radius + double max_friction_torque = 0; + double distWheels = 0; // distance between left and right wheels + size_t nWheels = 0; +}; + +static VehicleParams get_vehicle_params(const mvsim::VehicleBase& veh) +{ + VehicleParams p; + p.nWheels = veh.getNumWheels(); + p.total_mass = veh.getChassisMass(); + for (size_t i = 0; i < p.nWheels; i++) + p.total_mass += veh.getWheelInfo(i).mass; + + p.partial_mass = p.total_mass / static_cast(p.nWheels); + p.R = veh.getWheelInfo(0).diameter * 0.5; + + const double mu = 0.8; + p.max_friction_torque = mu * p.partial_mass * 9.81 * p.R; + + // Wheel track (for differential drive) + if (p.nWheels >= 2) + p.distWheels = std::abs(veh.getWheelInfo(0).y - veh.getWheelInfo(1).y); + + return p; +} + +// ======================================================== +// Plant identification from samples +// ======================================================== +static PlantModel identify_plant( + const std::vector& samples, double test_input, double sim_duration, + double sim_step, const char* label) +{ + PlantModel plant; + plant.torque_applied = test_input; + + if (samples.size() < 10) + { + std::fprintf(stderr, "Error: Too few samples recorded (%zu) for %s\n", samples.size(), label); + return plant; + } + + // Steady-state value (average of last 20%) + const size_t ss_start = samples.size() * 80 / 100; + double ss_sum = 0; + for (size_t i = ss_start; i < samples.size(); i++) + ss_sum += samples[i].value; + + plant.v_ss = ss_sum / static_cast(samples.size() - ss_start); + plant.K = plant.v_ss / test_input; + + // Time constant τ: time to reach 63.2% of v_ss + const double v_63 = plant.v_ss * 0.632; + plant.tau = sim_duration; // fallback + for (const auto& s : samples) + { + if (s.value >= v_63) + { + plant.tau = s.time; + break; + } + } + + // Sanity check: if tau is very small, use 95% method + if (plant.tau < 2 * sim_step) + { + const double v_95 = plant.v_ss * 0.95; + for (const auto& s : samples) + { + if (s.value >= v_95) + { + plant.tau = -s.time / std::log(0.05); + break; + } + } + } + + return plant; +} + +// ======================================================== +// Run open-loop step response with "raw" controller +// ======================================================== +static PlantModel run_open_loop_step( + const std::string& vehicle_xml_path, const std::string& vehicle_class, double test_torque, + double sim_duration, double sim_step, bool rotation_mode) +{ + auto [world, veh] = create_world_and_vehicle(vehicle_xml_path, vehicle_class, sim_step); // Replace the controller with "raw" for open-loop torque control if (auto* diffVeh = dynamic_cast(veh.get())) @@ -103,59 +198,43 @@ static PlantModel run_open_loop_step( THROW_EXCEPTION("Unsupported vehicle dynamics type for PID tuning."); } - world.insert_vehicle(veh); + world->insert_vehicle(veh); - // Print vehicle info - const size_t nWheels = veh->getNumWheels(); - std::printf("Vehicle: %s (%zu wheels)\n", vehicle_class.c_str(), nWheels); - for (size_t i = 0; i < nWheels; i++) - { - const auto& w = veh->getWheelInfo(i); - std::printf( - " Wheel %zu: pos=(%.3f, %.3f) R=%.3f mass=%.2f\n", i, w.x, w.y, w.diameter * 0.5, - w.mass); - } - std::printf(" Chassis mass: %.1f kg\n", veh->getChassisMass()); + auto params = get_vehicle_params(*veh); - // Compute friction limit for max_torque recommendation - // max_friction_force = mu * partial_mass * g - // max_torque = max_friction_force * R - double total_mass = veh->getChassisMass(); - for (size_t i = 0; i < nWheels; i++) + if (!rotation_mode) { - total_mass += veh->getWheelInfo(i).mass; + // Print vehicle info (only once, during linear test) + std::printf("Vehicle: %s (%zu wheels)\n", vehicle_class.c_str(), params.nWheels); + for (size_t i = 0; i < params.nWheels; i++) + { + const auto& w = veh->getWheelInfo(i); + std::printf( + " Wheel %zu: pos=(%.3f, %.3f) R=%.3f mass=%.2f\n", i, w.x, w.y, + w.diameter * 0.5, w.mass); + } + std::printf(" Chassis mass: %.1f kg\n", veh->getChassisMass()); + std::printf(" Wheel track: %.3f m\n", params.distWheels); + std::printf( + " Estimated max friction torque per wheel: %.2f Nm " + "(mu=0.8, partial_mass=%.1f kg, R=%.3f m)\n", + params.max_friction_torque, params.partial_mass, params.R); } - const double partial_mass = total_mass / static_cast(nWheels); - const double R = veh->getWheelInfo(0).diameter * 0.5; - const double mu = 0.8; // typical default - const double max_friction_force = mu * partial_mass * 9.81; - const double max_friction_torque = max_friction_force * R; - std::printf( - " Estimated max friction torque per wheel: %.2f Nm " - "(mu=%.1f, partial_mass=%.1f kg, R=%.3f m)\n", - max_friction_torque, mu, partial_mass, R); - - // Choose test torque: use a fraction of friction limit if not specified + // Choose test torque if (test_torque <= 0) - { - test_torque = max_friction_torque * 0.5; - } + test_torque = params.max_friction_torque * 0.5; - std::printf(" Test torque: %.2f Nm\n\n", test_torque); + std::printf(" Test torque: %.2f Nm (%s)\n\n", test_torque, rotation_mode ? "rotation" : "linear"); - // Setup data recording via CSVLogger callbacks - // We record the average velocity_x across all wheels + // Setup data recording std::vector samples; double current_avg_vel = 0; int wheel_count = 0; for (auto& logger : veh->getLoggers()) { - if (!logger) - { - continue; - } + if (!logger) continue; logger->setRecording(true); logger->setFileWritingEnabled(false); logger->registerOnRowCallback( @@ -170,23 +249,29 @@ static PlantModel run_open_loop_step( }); } - // Apply constant torque via raw controller - // The raw controller sets torques directly each step const int steps = static_cast(sim_duration / sim_step); for (int i = 0; i < steps; i++) { - // Set torques on all wheels via the raw controller - // For differential: set both L and R to same torque (straight) - // Motor torque sign convention: negative torque → forward motion + // For linear: both wheels get same torque (forward) + // For rotation: opposite torques (spin in place) if (auto* diffVeh = dynamic_cast(veh.get())) { auto* rawCtrl = dynamic_cast( diffVeh->getController().get()); if (rawCtrl) { - rawCtrl->setpoint_wheel_torque_l = -test_torque; - rawCtrl->setpoint_wheel_torque_r = -test_torque; + if (rotation_mode) + { + // Opposite torques: L forward, R backward → positive omega + rawCtrl->setpoint_wheel_torque_l = -test_torque; + rawCtrl->setpoint_wheel_torque_r = test_torque; + } + else + { + rawCtrl->setpoint_wheel_torque_l = -test_torque; + rawCtrl->setpoint_wheel_torque_r = -test_torque; + } } } else if (auto* ackVeh = dynamic_cast(veh.get())) @@ -195,83 +280,73 @@ static PlantModel run_open_loop_step( ackVeh->getController().get()); if (rawCtrl) { - rawCtrl->setpoint_wheel_torque_l = -test_torque; - rawCtrl->setpoint_wheel_torque_r = -test_torque; + if (rotation_mode) + { + rawCtrl->setpoint_wheel_torque_l = -test_torque; + rawCtrl->setpoint_wheel_torque_r = test_torque; + } + else + { + rawCtrl->setpoint_wheel_torque_l = -test_torque; + rawCtrl->setpoint_wheel_torque_r = -test_torque; + } rawCtrl->setpoint_steer_ang = 0; } } - // Reset accumulators current_avg_vel = 0; wheel_count = 0; - world.run_simulation(sim_step); + world->run_simulation(sim_step); - // Record average wheel velocity if (wheel_count > 0) { - samples.push_back({i * sim_step, current_avg_vel / wheel_count}); + if (rotation_mode) + { + // For rotation, measure the difference in wheel velocities + // The callback accumulates all wheel vels. For differential with 2 wheels: + // omega = (v_R - v_L) / distWheels + // But we measure via odometry estimate instead for robustness + // Actually, let's use the per-wheel approach: + // With opposite torques, L goes forward (+vel_x) and R goes backward (-vel_x) + // avg_vel_x will be ~0, but we want the differential. + // Better: read omega from the vehicle's velocity estimate. + } + else + { + samples.push_back({i * sim_step, current_avg_vel / wheel_count}); + } } - } - // ======================================================== - // Plant identification: fit first-order model v(t) = v_ss * (1 - e^(-t/τ)) - // ======================================================== - // Find steady-state velocity (average of last 20% of samples) - PlantModel plant; - plant.torque_applied = test_torque; - - if (samples.size() < 10) - { - std::fprintf(stderr, "Error: Too few samples recorded (%zu)\n", samples.size()); - return plant; + if (rotation_mode) + { + // Use vehicle's odometry estimate for angular velocity + mrpt::math::TTwist2D vel; + if (auto* diffVeh = dynamic_cast(veh.get())) + vel = diffVeh->getVelocityLocalOdoEstimate(); + else if (auto* ackVeh = dynamic_cast(veh.get())) + vel = ackVeh->getVelocityLocalOdoEstimate(); + + samples.push_back({i * sim_step, std::abs(vel.omega)}); + } } - const size_t ss_start = samples.size() * 80 / 100; - double v_ss_sum = 0; - for (size_t i = ss_start; i < samples.size(); i++) - { - v_ss_sum += samples[i].velocity_x; - } - plant.v_ss = v_ss_sum / static_cast(samples.size() - ss_start); - plant.K = plant.v_ss / test_torque; + const char* label = rotation_mode ? "rotation" : "linear"; + PlantModel plant = identify_plant(samples, test_torque, sim_duration, sim_step, label); - // Find time constant τ: time to reach 63.2% of v_ss - const double v_63 = plant.v_ss * 0.632; - plant.tau = sim_duration; // fallback - for (const auto& s : samples) + std::printf("=== Open-Loop Step Response Analysis (%s) ===\n", label); + std::printf("Applied torque: %.3f Nm\n", test_torque); + if (rotation_mode) { - if (s.velocity_x >= v_63) - { - plant.tau = s.time; - break; - } + std::printf("Steady-state omega: %.4f rad/s\n", plant.v_ss); + std::printf("Plant gain K: %.4f (rad/s)/Nm\n", plant.K); } - - // Sanity check: if tau is very small (< 2*sim_step), the system responds - // almost instantly (pure integrator with friction damping). Use a more - // robust method: fit via least-squares on the exponential. - if (plant.tau < 2 * sim_step) + else { - // Use the time to reach 95% of v_ss instead, and derive τ from that - // v(t_95) = v_ss * (1 - e^(-t_95/τ)) = 0.95 * v_ss - // => τ = -t_95 / ln(0.05) - const double v_95 = plant.v_ss * 0.95; - for (const auto& s : samples) - { - if (s.velocity_x >= v_95) - { - plant.tau = -s.time / std::log(0.05); - break; - } - } + std::printf("Steady-state velocity: %.4f m/s\n", plant.v_ss); + std::printf("Plant gain K: %.4f (m/s)/Nm\n", plant.K); } - - std::printf("=== Open-Loop Step Response Analysis ===\n"); - std::printf("Applied torque: %.3f Nm\n", test_torque); - std::printf("Steady-state velocity: %.4f m/s\n", plant.v_ss); - std::printf("Plant gain K: %.4f (m/s)/Nm\n", plant.K); - std::printf("Time constant τ: %.4f s\n", plant.tau); + std::printf("Time constant tau: %.4f s\n", plant.tau); return plant; } @@ -286,21 +361,17 @@ static PIDParams compute_pid_imc( // IMC tuning for first-order plant G(s) = K / (τs + 1): // Desired closed-loop time constant: τ_cl = τ * aggressiveness - // (smaller aggressiveness = faster response) - // - // PI controller: C(s) = KP * (1 + 1/(Ti*s)) + // PI controller: // KP = τ / (K * τ_cl) - // Ti = τ (integral time = plant time constant → pole-zero cancellation) - // KI = KP / Ti = 1 / (K * τ_cl) + // KI = KP / τ = 1 / (K * τ_cl) + // KD = 0 const double tau_cl = plant.tau * aggressiveness; pid.KP = plant.tau / (plant.K * tau_cl); - pid.KI = pid.KP / plant.tau; // = 1 / (K * tau_cl) - pid.KD = 0; // not needed for first-order plants + pid.KI = pid.KP / plant.tau; + pid.KD = 0; - // max_torque: set to ~80% of friction limit to avoid wheel slip, - // but at least enough for the PID to be effective pid.max_torque = max_friction_torque * 0.8; return pid; @@ -311,44 +382,23 @@ static PIDParams compute_pid_imc( // ======================================================== struct ValidationResult { - double rise_time = 0; // time to reach 90% of setpoint - double settling_time = 0; // time to reach and stay within 2% of setpoint - double overshoot_pct = 0; // percent overshoot + double rise_time = 0; + double settling_time = 0; + double overshoot_pct = 0; double steady_state_error = 0; - double stop_settling_time = 0; // time to stop after step-down - double stop_overshoot = 0; // velocity undershoot during stop (should be ~0) + double stop_settling_time = 0; + double stop_overshoot = 0; }; static ValidationResult validate_pid( const std::string& vehicle_xml_path, const std::string& vehicle_class, const PIDParams& pid, - double sim_step) + double sim_step, bool rotation_mode) { ValidationResult result; - mvsim::World world; - world.headless(true); - world.set_gravity(9.81); - world.set_simul_timestep(sim_step); + auto [world, veh] = create_world_and_vehicle(vehicle_xml_path, vehicle_class, sim_step); - { - std::string world_xml = - "" - " " - ""; - world.load_from_XML(world_xml, vehicle_xml_path); - } - - // Create vehicle from class definition (inherits its controller) - std::string veh_xml = "" - " 0 0 0" - ""; - - auto veh = mvsim::VehicleBase::factory(&world, veh_xml); - - // Programmatically set the proposed PID parameters on the controller + // Set proposed PID parameters on the controller if (auto* diffVeh = dynamic_cast(veh.get())) { auto pidCtrl = std::make_shared(*diffVeh); @@ -369,76 +419,54 @@ static ValidationResult validate_pid( ackVeh->getController() = pidCtrl; } - world.insert_vehicle(veh); - - // Record wheel velocity - std::vector samples; - double current_avg_vel = 0; - int wheel_count = 0; + world->insert_vehicle(veh); - for (auto& logger : veh->getLoggers()) - { - if (!logger) - { - continue; - } - logger->setRecording(true); - logger->setFileWritingEnabled(false); - logger->registerOnRowCallback( - [¤t_avg_vel, &wheel_count](const std::map& cols) - { - auto it = cols.find(mvsim::VehicleBase::WL_VEL_X); - if (it != cols.end()) - { - current_avg_vel += it->second; - wheel_count++; - } - }); - } - - // Phase 1: Step-up to 1.0 m/s for 3 seconds + // Phase 1: Step-up, Phase 2: Step-down + // For linear: 1.0 m/s + // For rotation: 1.0 rad/s (~57 deg/s) const double setpoint = 1.0; const double step_up_time = 3.0; const double step_down_time = 3.0; - const mrpt::math::TTwist2D cmd_forward(setpoint, 0, 0); + + const mrpt::math::TTwist2D cmd_active = + rotation_mode ? mrpt::math::TTwist2D(0, 0, setpoint) : mrpt::math::TTwist2D(setpoint, 0, 0); const mrpt::math::TTwist2D cmd_stop(0, 0, 0); - veh->getControllerInterface()->setTwistCommand(cmd_forward); + veh->getControllerInterface()->setTwistCommand(cmd_active); int total_steps = static_cast((step_up_time + step_down_time) / sim_step); int step_down_at = static_cast(step_up_time / sim_step); - double max_vel = 0; + double max_val = 0; + + std::vector samples; for (int i = 0; i < total_steps; i++) { if (i == step_down_at) - { veh->getControllerInterface()->setTwistCommand(cmd_stop); - } - current_avg_vel = 0; - wheel_count = 0; - world.run_simulation(sim_step); + world->run_simulation(sim_step); + + // Read actual velocity + mrpt::math::TTwist2D vel; + if (auto* diffVeh = dynamic_cast(veh.get())) + vel = diffVeh->getVelocityLocalOdoEstimate(); + else if (auto* ackVeh = dynamic_cast(veh.get())) + vel = ackVeh->getVelocityLocalOdoEstimate(); - double avg_v = (wheel_count > 0) ? current_avg_vel / wheel_count : 0; - samples.push_back({i * sim_step, avg_v}); + double measured = rotation_mode ? vel.omega : vel.vx; + samples.push_back({i * sim_step, measured}); if (i < step_down_at) - { - max_vel = std::max(max_vel, avg_v); - } + max_val = std::max(max_val, measured); } - // Analyze step-up response // Rise time: time to reach 90% of setpoint - result.rise_time = step_up_time; // fallback: never reached + result.rise_time = step_up_time; for (const auto& s : samples) { - if (s.time > step_up_time) - { - break; - } - if (s.velocity_x >= 0.9 * setpoint) + if (s.time > step_up_time) break; + if (s.value >= 0.9 * setpoint) { result.rise_time = s.time; break; @@ -446,63 +474,112 @@ static ValidationResult validate_pid( } // Overshoot - result.overshoot_pct = std::max(0.0, (max_vel - setpoint) / setpoint * 100.0); + result.overshoot_pct = std::max(0.0, (max_val - setpoint) / setpoint * 100.0); - // Steady-state error (average of last 20% before step-down) + // Steady-state error { const size_t ss_start = step_down_at * 80 / 100; double ss_sum = 0; int ss_count = 0; for (size_t i = ss_start; i < static_cast(step_down_at); i++) { - ss_sum += samples[i].velocity_x; + ss_sum += samples[i].value; ss_count++; } if (ss_count > 0) - { result.steady_state_error = std::abs(setpoint - ss_sum / ss_count); - } } - // Settling time during step-up: time to enter and stay within 2% band + // Settling time { const double band = 0.02 * setpoint; - result.settling_time = step_up_time; // fallback - // Scan backwards from step_down to find last exit from band + result.settling_time = step_up_time; for (int i = step_down_at - 1; i >= 0; i--) { - if (std::abs(samples[i].velocity_x - setpoint) > band) + if (std::abs(samples[i].value - setpoint) > band) { if (i + 1 < step_down_at) - { result.settling_time = samples[i + 1].time; - } break; } } } - // Analyze step-down (stop) response + // Step-down analysis { - // Find settling time to reach <2% of setpoint const double stop_band = 0.02 * setpoint; result.stop_settling_time = step_down_time; - double min_vel = setpoint; + double min_val = setpoint; for (size_t i = step_down_at; i < samples.size(); i++) { - min_vel = std::min(min_vel, samples[i].velocity_x); - if (std::abs(samples[i].velocity_x) <= stop_band) + min_val = std::min(min_val, samples[i].value); + if (std::abs(samples[i].value) <= stop_band) { result.stop_settling_time = samples[i].time - step_up_time; break; } } - result.stop_overshoot = std::max(0.0, -min_vel); // negative velocity = rebound + result.stop_overshoot = std::max(0.0, -min_val); } return result; } +// ======================================================== +// Print validation results +// ======================================================== +static bool print_validation( + const ValidationResult& val, const char* label, double setpoint, const char* unit) +{ + std::printf("\nStep-up (0 -> %.1f %s):\n", setpoint, unit); + std::printf(" Rise time (90%%): %.3f s\n", val.rise_time); + std::printf(" Settling time (2%%): %.3f s\n", val.settling_time); + std::printf(" Overshoot: %.1f %%\n", val.overshoot_pct); + std::printf(" Steady-state error: %.4f %s\n", val.steady_state_error, unit); + + std::printf("\nStep-down (%.1f -> 0 %s):\n", setpoint, unit); + std::printf(" Settling time (2%%): %.3f s\n", val.stop_settling_time); + if (val.stop_overshoot > 0.001) + { + std::printf( + " Rebound: %.4f %s *** WARNING: vehicle reverses!\n", val.stop_overshoot, unit); + } + else + { + std::printf(" Rebound: none\n"); + } + + bool good = true; + if (val.overshoot_pct > 20) + { + std::printf( + "WARNING [%s]: High overshoot (%.1f%%). Consider increasing aggressiveness.\n", label, + val.overshoot_pct); + good = false; + } + if (val.steady_state_error > 0.05) + { + std::printf( + "WARNING [%s]: Large steady-state error (%.4f). KI may need to be higher.\n", label, + val.steady_state_error); + good = false; + } + if (val.stop_overshoot > 0.01) + { + std::printf( + "WARNING [%s]: Vehicle rebounds when stopping. KI may be too high.\n", label); + good = false; + } + if (val.rise_time > 2.0) + { + std::printf( + "WARNING [%s]: Slow rise time (%.1fs). Consider decreasing aggressiveness.\n", label, + val.rise_time); + good = false; + } + return good; +} + // ======================================================== // Main // ======================================================== @@ -510,47 +587,32 @@ int main(int argc, char** argv) { try { - TCLAP::CmdLine cmd("mvsim-pid-tuner: Automatic PID tuning for MVSim vehicles", ' '); + CLI::App app("mvsim-pid-tuner: Automatic PID tuning for MVSim vehicles"); - TCLAP::UnlabeledValueArg argVehicleXml( - "vehicle_xml", "Path to vehicle definition XML file (e.g. small_robot.vehicle.xml)", - true, "", "vehicle.xml", cmd); + std::string vehicle_xml; + app.add_option("vehicle_xml", vehicle_xml, "Path to vehicle definition XML file") + ->required(); - TCLAP::UnlabeledValueArg argVehicleClass( - "vehicle_class", "Vehicle class name as defined in the XML (e.g. small_robot)", true, - "", "class_name", cmd); + std::string vehicle_class; + app.add_option("vehicle_class", vehicle_class, "Vehicle class name as defined in the XML") + ->required(); - TCLAP::ValueArg argTorque( - "t", "torque", + double test_torque = -1.0; + app.add_option("-t,--torque", test_torque, "Test torque (Nm) for open-loop identification. " - "Default: 50% of estimated friction limit.", - false, -1.0, "Nm", cmd); + "Default: 50% of estimated friction limit."); - TCLAP::ValueArg argDuration( - "d", "duration", "Open-loop step duration (s). Default: 5.0", false, 5.0, "seconds", - cmd); + double sim_duration = 5.0; + app.add_option("-d,--duration", sim_duration, "Open-loop step duration (s)."); - TCLAP::ValueArg argStep( - "s", "sim-step", "Simulation time step (s). Default: 0.001", false, 0.001, "seconds", - cmd); + double sim_step = 0.001; + app.add_option("-s,--sim-step", sim_step, "Simulation time step (s)."); - TCLAP::ValueArg argAggressiveness( - "a", "aggressiveness", - "Closed-loop aggressiveness factor (0.1=very aggressive, " - "1.0=conservative). Default: 0.25", - false, 0.25, "factor", cmd); - - if (!cmd.parse(argc, argv)) - { - return 0; // Help/version was printed - } + double aggressiveness = 0.25; + app.add_option("-a,--aggressiveness", aggressiveness, + "Closed-loop aggressiveness factor (0.1=very aggressive, 1.0=conservative)."); - const std::string vehicle_xml = argVehicleXml.getValue(); - const std::string vehicle_class = argVehicleClass.getValue(); - const double test_torque = argTorque.getValue(); - const double sim_duration = argDuration.getValue(); - const double sim_step = argStep.getValue(); - const double aggressiveness = argAggressiveness.getValue(); + CLI11_PARSE(app, argc, argv); if (aggressiveness <= 0) { @@ -563,59 +625,57 @@ int main(int argc, char** argv) " MVSim PID Auto-Tuner\n" "======================================\n\n"); - // Phase 1: Open-loop identification - std::printf("--- Phase 1: Open-loop plant identification ---\n"); - PlantModel plant = - run_open_loop_step(vehicle_xml, vehicle_class, test_torque, sim_duration, sim_step); + // Phase 1a: Open-loop linear identification + std::printf("--- Phase 1a: Open-loop LINEAR plant identification ---\n"); + PlantModel plantLin = run_open_loop_step( + vehicle_xml, vehicle_class, test_torque, sim_duration, sim_step, false); - if (plant.K <= 0 || plant.tau <= 0) + if (plantLin.K <= 0 || plantLin.tau <= 0) { - std::fprintf(stderr, "\nError: Could not identify plant model. Check vehicle XML.\n"); + std::fprintf(stderr, "\nError: Could not identify linear plant model.\n"); return 1; } - // Compute friction limit for max_torque - // Re-derive from plant data - // We can estimate: max_friction_torque ≈ v_ss / K_velocity_per_force * R - // But simpler: just use the same estimation as in the open-loop test - // For that we need vehicle params again. Let's just load them. - mvsim::World tmpWorld; - tmpWorld.headless(true); - tmpWorld.set_gravity(9.81); - tmpWorld.set_simul_timestep(sim_step); - { - std::string world_xml = - "" - " " - ""; - tmpWorld.load_from_XML(world_xml, vehicle_xml); - } + // Phase 1b: Open-loop rotation identification + std::printf("\n--- Phase 1b: Open-loop ROTATION plant identification ---\n"); + PlantModel plantRot = run_open_loop_step( + vehicle_xml, vehicle_class, test_torque, sim_duration, sim_step, true); - auto tmpVeh = mvsim::VehicleBase::factory( - &tmpWorld, "" - " 0 0 0" - ""); - - double total_mass = tmpVeh->getChassisMass(); - for (size_t i = 0; i < tmpVeh->getNumWheels(); i++) + if (plantRot.K <= 0 || plantRot.tau <= 0) { - total_mass += tmpVeh->getWheelInfo(i).mass; + std::fprintf(stderr, "\nError: Could not identify rotation plant model.\n"); + return 1; } - const double partial_mass = total_mass / static_cast(tmpVeh->getNumWheels()); - const double R = tmpVeh->getWheelInfo(0).diameter * 0.5; - const double mu = 0.8; - const double max_friction_torque = mu * partial_mass * 9.81 * R; + // Get vehicle params for max_torque computation + auto [tmpWorld, tmpVeh] = + create_world_and_vehicle(vehicle_xml, vehicle_class, sim_step); + auto vehParams = get_vehicle_params(*tmpVeh); // Phase 2: Compute PID parameters + // Since the twist_pid controller uses the same KP/KI/KD for both wheels, + // and rotation dynamics differ from linear dynamics, we need to find + // parameters that work well for both. + // + // Strategy: compute PID for both linear and rotation plants, then pick + // the more conservative (slower) parameters to ensure stability for both. std::printf("\n--- Phase 2: IMC PID tuning (aggressiveness=%.2f) ---\n", aggressiveness); - PIDParams pid = compute_pid_imc(plant, max_friction_torque, aggressiveness); + PIDParams pidLin = compute_pid_imc(plantLin, vehParams.max_friction_torque, aggressiveness); + PIDParams pidRot = compute_pid_imc(plantRot, vehParams.max_friction_torque, aggressiveness); - std::printf("\nProposed PID parameters:\n"); + std::printf("\n Linear-only PID: KP=%.4f KI=%.4f\n", pidLin.KP, pidLin.KI); + std::printf(" Rotation-only PID: KP=%.4f KI=%.4f\n", pidRot.KP, pidRot.KI); + + // Use the geometric mean of both to balance linear and rotation performance. + // This ensures neither mode is overly aggressive. + PIDParams pid; + pid.KP = std::sqrt(pidLin.KP * pidRot.KP); + pid.KI = std::sqrt(pidLin.KI * pidRot.KI); + pid.KD = 0; + pid.max_torque = vehParams.max_friction_torque * 0.8; + + std::printf("\nProposed PID parameters (balanced linear + rotation):\n"); std::printf(" KP: %.4f\n", pid.KP); std::printf(" KI: %.4f\n", pid.KI); std::printf(" KD: %.4f\n", pid.KD); @@ -630,64 +690,24 @@ int main(int argc, char** argv) // Phase 3: Validate with closed-loop simulation std::printf("\n--- Phase 3: Closed-loop validation ---\n"); - std::printf("Running step-up (1.0 m/s, 3s) then step-down (stop, 3s)...\n"); - - ValidationResult val = validate_pid(vehicle_xml, vehicle_class, pid, sim_step); - std::printf("\nStep-up (0 → 1.0 m/s):\n"); - std::printf(" Rise time (90%%): %.3f s\n", val.rise_time); - std::printf(" Settling time (2%%): %.3f s\n", val.settling_time); - std::printf(" Overshoot: %.1f %%\n", val.overshoot_pct); - std::printf(" Steady-state error: %.4f m/s\n", val.steady_state_error); + std::printf("\n[Linear] Step-up (1.0 m/s, 3s) then step-down (stop, 3s)...\n"); + ValidationResult valLin = validate_pid(vehicle_xml, vehicle_class, pid, sim_step, false); + bool goodLin = print_validation(valLin, "linear", 1.0, "m/s"); - std::printf("\nStep-down (1.0 → 0 m/s):\n"); - std::printf(" Settling time (2%%): %.3f s\n", val.stop_settling_time); - if (val.stop_overshoot > 0.001) - { - std::printf( - " Rebound (neg velocity): %.4f m/s *** WARNING: vehicle reverses!\n", - val.stop_overshoot); - } - else - { - std::printf(" Rebound: none\n"); - } + std::printf("\n[Rotation] Step-up (1.0 rad/s, 3s) then step-down (stop, 3s)...\n"); + ValidationResult valRot = validate_pid(vehicle_xml, vehicle_class, pid, sim_step, true); + bool goodRot = print_validation(valRot, "rotation", 1.0, "rad/s"); - // Quality assessment std::printf("\n--- Assessment ---\n"); - bool good = true; - if (val.overshoot_pct > 20) - { - std::printf( - "WARNING: High overshoot (%.1f%%). Consider increasing aggressiveness factor.\n", - val.overshoot_pct); - good = false; - } - if (val.steady_state_error > 0.05) - { - std::printf( - "WARNING: Large steady-state error (%.4f). KI may need to be higher.\n", - val.steady_state_error); - good = false; - } - if (val.stop_overshoot > 0.01) - { - std::printf( - "WARNING: Vehicle rebounds when stopping. KI may be too high " - "or max_torque too large.\n"); - good = false; - } - if (val.rise_time > 2.0) - { - std::printf( - "WARNING: Slow rise time (%.1fs). Consider decreasing aggressiveness factor.\n", - val.rise_time); - good = false; - } - if (good) - { - std::printf("All metrics look good!\n"); - } + if (goodLin && goodRot) + std::printf("All metrics look good for both linear and rotation!\n"); + else if (goodLin) + std::printf("Linear metrics OK, but rotation has warnings (see above).\n"); + else if (goodRot) + std::printf("Rotation metrics OK, but linear has warnings (see above).\n"); + else + std::printf("Both linear and rotation have warnings (see above).\n"); std::printf("\n"); return 0; diff --git a/package.xml b/package.xml index 23e47042..109be233 100644 --- a/package.xml +++ b/package.xml @@ -22,13 +22,13 @@ ament_cmake - boost + cli11 libzmq3-dev - mrpt_libgui - mrpt_libmaps - mrpt_libposes + mrpt_gui mrpt_libros_bridge - mrpt_libtclap + mrpt_maps + mrpt_topography + mrpt_tfest nav_msgs protobuf-dev pybind11-dev @@ -37,8 +37,8 @@ python3-venv sensor_msgs stereo_msgs - tf2 tf2_geometry_msgs + tf2 unzip visualization_msgs wget diff --git a/tests/test_shape2p5.cpp b/tests/test_shape2p5.cpp index 5df713ea..42f51343 100644 --- a/tests/test_shape2p5.cpp +++ b/tests/test_shape2p5.cpp @@ -7,11 +7,11 @@ | See COPYING | +-------------------------------------------------------------------------+ */ -#include -#include -#include #include // mrpt::system::pathJoin() #include +#include +#include +#include #include #include @@ -26,7 +26,7 @@ void shape_test_sphere() { auto& csc = CollisionShapeCache::Instance(); const float radius = 0.5f; - auto glSphere = mrpt::opengl::CSphere::Create(radius); + auto glSphere = mrpt::viz::CSphere::Create(radius); const Shape2p5 shape = csc.get(*glSphere, -radius, +radius, {}, 1.0f); @@ -38,7 +38,7 @@ void shape_test_cylinder() auto& csc = CollisionShapeCache::Instance(); const float radius = 0.5f, L = 2.0f; - auto glCyl = mrpt::opengl::CCylinder::Create(radius, radius, L); + auto glCyl = mrpt::viz::CCylinder::Create(radius, radius, L); const Shape2p5 shape = csc.get(*glCyl, 0, L, {}, 1.0f); @@ -50,7 +50,7 @@ void shape_test_merge() auto& csc = CollisionShapeCache::Instance(); const float radius = 0.5f, L = 2.0f; - auto glCyl = mrpt::opengl::CCylinder::Create(radius, radius, L); + auto glCyl = mrpt::viz::CCylinder::Create(radius, radius, L); const Shape2p5 s1 = csc.get(*glCyl, 0, L, mrpt::poses::CPose3D::FromTranslation(-0.15, 0, 0), 1.0f); @@ -70,7 +70,7 @@ void shape_test_simplecamera() { auto& csc = CollisionShapeCache::Instance(); - auto glModel = mrpt::opengl::CAssimpModel::Create(); + auto glModel = mrpt::viz::CAssimpModel::Create(); glModel->loadScene(mrpt::system::pathJoin({MVSIM_TEST_DIR, "../models/simple_camera.dae"})); const Shape2p5 shape = csc.get(*glModel, 0, 1.0, {}, 1.0f);