patrykcieslak · Michele1996 · Oct 30, 2024 · Oct 30, 2024 · Oct 31, 2024 · Oct 31, 2024
diff --git a/Library/include/StonefishCommon.h b/Library/include/StonefishCommon.h
diff --git a/Library/include/actuators/Actuator.h b/Library/include/actuators/Actuator.h
diff --git a/Library/include/actuators/ActuatorDynamics.h b/Library/include/actuators/ActuatorDynamics.h
diff --git a/Library/include/actuators/DCMotor.h b/Library/include/actuators/DCMotor.h
diff --git a/Library/include/actuators/JointActuator.h b/Library/include/actuators/JointActuator.h
diff --git a/Library/include/actuators/Light.h b/Library/include/actuators/Light.h
@@ -103,6 +103,8 @@ namespace sf
         //! A method returning the type of the actuator.
         ActuatorType getType() const;
 
+        OpenGLLight* getGLLight();
+
     private:
         void InitGraphics();
 

diff --git a/Library/include/actuators/LinkActuator.h b/Library/include/actuators/LinkActuator.h
diff --git a/Library/include/actuators/Motor.h b/Library/include/actuators/Motor.h
diff --git a/Library/include/actuators/Propeller.h b/Library/include/actuators/Propeller.h
diff --git a/Library/include/actuators/Push.h b/Library/include/actuators/Push.h
diff --git a/Library/include/actuators/Rudder.h b/Library/include/actuators/Rudder.h
diff --git a/Library/include/actuators/Servo.h b/Library/include/actuators/Servo.h
diff --git a/Library/include/actuators/SimpleThruster.h b/Library/include/actuators/SimpleThruster.h
diff --git a/Library/include/actuators/SuctionCup.h b/Library/include/actuators/SuctionCup.h
diff --git a/Library/include/actuators/Thruster.h b/Library/include/actuators/Thruster.h
diff --git a/Library/include/actuators/VariableBuoyancy.h b/Library/include/actuators/VariableBuoyancy.h
diff --git a/Library/include/comms/AcousticModem.h b/Library/include/comms/AcousticModem.h
diff --git a/Library/include/comms/Comm.h b/Library/include/comms/Comm.h
@@ -29,24 +29,39 @@
 #include <SDL2/SDL_mutex.h>
 #include <deque>
 #include "StonefishCommon.h"
+#include <variant>
 
 namespace sf
 {
     //! An enum defining types of comms.
-    enum class CommType {RADIO, ACOUSTIC, USBL, VLC};
+    enum class CommType {RADIO, ACOUSTIC, USBL, VLC, OPTIC};
 
     struct Renderable;
     class Entity;
     class StaticEntity;
     class MovingEntity;
 
+struct Point {
+    float x, y, z;
+    float intensity;
+};
+
+struct PointCloud {
+    std::vector<Point> points;
+    std::string frame_id;
+    double timestamp;
+};
+
     struct CommDataFrame
     {
+        using DataType = std::variant<int, float, double, std::string, std::vector<uint8_t>, std::vector<float>, std::vector<double>, PointCloud>;
+
         Scalar timeStamp;
         uint64_t seq;
         uint64_t source;
         uint64_t destination;
         std::string data;
+        DataType data_mission;
     };
 
     //! An abstract class representing a communication device.

diff --git a/Library/include/comms/USBL.h b/Library/include/comms/USBL.h
diff --git a/Library/include/comms/USBLReal.h b/Library/include/comms/USBLReal.h
diff --git a/Library/include/comms/USBLSimple.h b/Library/include/comms/USBLSimple.h
diff --git a/Library/include/comms/VLC.h b/Library/include/comms/VLC.h
@@ -0,0 +1,127 @@
+/*    
+    This file is a part of Stonefish.
+
+    Stonefish is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    Stonefish is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+//
+//  VLC.h
+//  Stonefish
+
+#ifndef __Stonefish_VLC__
+#define __Stonefish_VLC__
+
+#include "actuators/Light.h"
+#include "comms/Comm.h"
+#include "graphics/OpenGLDataStructs.h"
+#include <map>
+
+namespace sf
+{
+    class Light;
+	class StaticEntity;
+    class AnimatedEntity;
+
+    //! A class representing a light of two common types: omni and spot.
+    class VLC : public Comm
+    {
+    public:
+        //! A constructor of an omni light.
+        /*!
+         \param uniqueName a name of the light
+		 \param radius a radius of the light source [m]
+         \param color a color of the light
+         \param lum the luminous power of the light [lm]
+         */
+        VLC(std::string uniqueName, uint64_t deviceId, Scalar minVerticalFOVDeg, Scalar maxVerticalFOVDeg, Scalar range, Scalar comm_speed);
+
+        void SendMessage(std::string data, const CommDataFrame::DataType& data_mission);
+        //! A constructor of a spot light.
+        /*!
+         \param uniqueName a name of the light
+		 \param radius a radius of the light source [m]
+         \param coneAngleDeg a cone angle of the spot light in degrees [deg]
+         \param color a color of the light
+         \param lum the luminous power of the light [lm]
+         */
+
+		//! A method used to attach the comm device to the world origin.
+        /*!
+         \param origin the place where the comm should be attached in the world frame
+         */
+        /*!
+         \param dt a time step of the simulation
+         */
+        virtual void InternalUpdate(Scalar dt);
+
+        //! A method implementing the rendering of the light dummy.
+        std::vector<Renderable> Render();
+
+        //! A method returning the type of the actuator.
+        CommType getType() const;
+
+        std::vector<Light*> getLights();
+
+        void SwitchOff();
+
+        void SwitchOn();
+
+        bool isActive();
+
+        void enable(bool t);
+
+        Scalar getRange();
+
+        void setRange(Scalar r);
+
+        Scalar getCommSpeed();
+
+        void setCommSpeed(Scalar r);
+
+        void addLight(Light* l);
+
+        bool getOcclusionTest() const;
+        CommDataFrame* getData();
+        void setWaterType(Scalar t);
+
+
+
+    protected:
+
+        virtual void ProcessMessages();
+        static VLC* getNode(uint64_t deviceId);
+
+    private:
+
+	Scalar R;
+        Scalar Fi;
+        Scalar coneAngle;
+        std::vector<Light*> lights;
+        Scalar minFov2, maxFov2;
+        bool active;
+        Scalar range; 
+        Scalar comm_speed;
+        static std::vector<uint64_t> getNodeIds();
+        static bool mutualContact(uint64_t device1Id, uint64_t device2Id);
+        bool isReceptionPossible(Vector3 dir, Scalar distance);
+        bool occlusion;
+        static std::map<uint64_t, VLC*> nodes;
+        CommDataFrame* data;
+        Scalar water_type;
+
+
+    };
+}
+
+#endif
diff --git a/Library/include/core/Battery.h b/Library/include/core/Battery.h
@@ -0,0 +1,40 @@
+#ifndef BATTERY_H
+#define BATTERY_H
+
+#include <vector>
+#include "sensors/Sensor.h"
+#include <iostream>
+
+namespace sf
+{
+
+class Battery
+{
+private:
+    float voltage;         // Battery voltage in volts
+    float capacity;        // Battery capacity in ampere-hours (Ah)
+    float maxCapacity;     // Maximum battery capacity in ampere-hours
+    double powerDraw;       // Current power draw in watts (calculated)
+    double energyRemaining; // Energy remaining in watt-hours (Wh)
+
+public:
+    Battery(float voltage, float capacity);
+
+    // Copy Constructor
+    Battery(const Battery& other);
+
+    // Assignment Operator
+    Battery& operator=(const Battery& other);
+
+    float getVoltage() const;
+    float getCapacity() const;
+    double getEnergyRemaining() const;
+    void consume(const std::vector<Sensor*>& sensors, float timeStep); // timeStep in seconds
+    void setVoltage(Scalar v);
+    void setMaxCapacity(Scalar mc);
+};
+
+} // namespace sf
+
+#endif // BATTERY_H
+
diff --git a/Library/include/core/Console.h b/Library/include/core/Console.h
diff --git a/Library/include/core/ConsoleSimulationApp.h b/Library/include/core/ConsoleSimulationApp.h
diff --git a/Library/include/core/FeatherstoneRobot.h b/Library/include/core/FeatherstoneRobot.h
diff --git a/Library/include/core/FilteredCollisionDispatcher.h b/Library/include/core/FilteredCollisionDispatcher.h
diff --git a/Library/include/core/GeneralRobot.h b/Library/include/core/GeneralRobot.h
diff --git a/Library/include/core/GraphicalSimulationApp.h b/Library/include/core/GraphicalSimulationApp.h
diff --git a/Library/include/core/MaterialManager.h b/Library/include/core/MaterialManager.h
diff --git a/Library/include/core/NED.h b/Library/include/core/NED.h
diff --git a/Library/include/core/NameManager.h b/Library/include/core/NameManager.h
diff --git a/Library/include/core/Robot.h b/Library/include/core/Robot.h
@@ -29,6 +29,7 @@
 #include <utility>
 #include "StonefishCommon.h"
 #include "joints/Joint.h"
+#include "Battery.h"
 
 namespace sf
 {
@@ -82,6 +83,21 @@ namespace sf
         void DefineRevoluteJoint(std::string jointName, std::string parentName, std::string childName, const Transform& origin, 
                                  const Vector3& axis, std::pair<Scalar, Scalar> positionLimits = std::make_pair(Scalar(1), Scalar(-1)), Scalar damping = Scalar(-1));
 
+        //! A method used to define a spherical joint between two mechanical parts of the robot.
+        /*!
+         \param jointName a name for the joint
+         \param parentName a name of the parent link
+         \param childName a name of the child link
+         \param origin frame of the joint
+         \param axis an axis of the joint
+         \param positionLimits a pair of min and max limit of joint position (if min > max then joint has no limits)
+         \param damping joint motion damping (works when there is no actuator attached to the joint) 
+         */
+        void DefineSphericalJoint(std::string jointName, std::string parentName, std::string childName, const Transform& origin, 
+                                 const Vector3& axis, std::pair<Scalar, Scalar> positionLimits = std::make_pair(Scalar(1), Scalar(-1)), Scalar damping = Scalar(-1));
+
+
+
         //! A method used to define a prismatic joint between two mechanical parts of the robot.
         /*!
          \param jointName a name for the joint
@@ -138,7 +154,7 @@ namespace sf
          \param actuatedLinkName a name of the link which is to be actuated
          \param origin a transformation from the link origin to the actuator frame
          */
-        virtual void AddLinkActuator(LinkActuator* a, const std::string& actuatedLinkName, const Transform& origin);
+        void AddLinkActuator(LinkActuator* a, const std::string& actuatedLinkName, const Transform& origin);
 
         //! A method used to attach an actuator to a specified joint of the robot.
         /*!
@@ -156,6 +172,14 @@ namespace sf
          */
         void AddComm(Comm* c, const std::string& attachmentLinkName, const Transform& origin);
 
+        //VISUAL EFFECTS
+        //! A method used to attach a visual to a specified link of the robot.
+        /*!
+         \param v a pointer to a visual object
+         \param attachmentLinkName a name of the link to which the visual is attached
+         \param origin a transformation from the link origin to the visual frame
+         */
+
         //GENERAL
         //! A method adding the robot to the simulation world (includes consistency checking).
         /*!
@@ -212,7 +236,14 @@ namespace sf
          \return a pointer to the comm object
          */
         Comm* getComm(size_t index);
-
+
+
+        //! A method returning a pointer to the visual by index.
+        /*!
+         \param index the id of the visual
+         \return a pointer to the visual object
+         */
+
         //! A method returning a pointer to the base link solid.
         SolidEntity* getBaseLink();
 
@@ -239,6 +270,22 @@ namespace sf
         //! A method returning type of algorithm used for the robot.
         virtual RobotType getType() const = 0;
 
+        Battery* getBattery();
+
+        void setBattery(Battery* b);
+
+        unsigned short getPort();
+
+        std::string getIPaddress();
+
+        void setPort(unsigned short p);
+
+        void setIPaddress(std::string ip);
+
+
+        std::vector<Sensor*> getSensors();
+        std::vector<Actuator*> getActuators();
+
     protected:
         struct JointData
         {
@@ -260,6 +307,9 @@ namespace sf
         std::vector<Comm*> comms;
         std::string name;
         bool fixed;
+        Battery* battery;
+        unsigned short port;
+        std::string ip_address;
     };
 }
 

diff --git a/Library/include/core/ScenarioParser.h b/Library/include/core/ScenarioParser.h
@@ -37,6 +37,7 @@ namespace sf
 {
     class SimulationManager;
     class Robot;
+    class Battery;
     class Entity;
     class SolidEntity;
     class Sensor;
@@ -279,7 +280,8 @@ namespace sf
          \return full file path
          */
         std::string GetFullPath(const std::string& path);
-
+
+	virtual bool ParseBattery(XMLElement* element, Battery* battery);
         //! A method informing if the simulation is working in graphical mode.
         bool isGraphicalSim();
 

diff --git a/Library/include/core/ScrewConstraint.h b/Library/include/core/ScrewConstraint.h
diff --git a/Library/include/core/SimulationApp.h b/Library/include/core/SimulationApp.h
diff --git a/Library/include/core/SimulationManager.h b/Library/include/core/SimulationManager.h
diff --git a/Library/include/core/TorusShape.h b/Library/include/core/TorusShape.h
diff --git a/Library/include/entities/AnimatedEntity.h b/Library/include/entities/AnimatedEntity.h
diff --git a/Library/include/entities/Entity.h b/Library/include/entities/Entity.h
diff --git a/Library/include/entities/FeatherstoneEntity.h b/Library/include/entities/FeatherstoneEntity.h
diff --git a/Library/include/entities/ForcefieldEntity.h b/Library/include/entities/ForcefieldEntity.h
diff --git a/Library/include/entities/MovingEntity.h b/Library/include/entities/MovingEntity.h
diff --git a/Library/include/entities/SolidEntity.h b/Library/include/entities/SolidEntity.h
diff --git a/Library/include/entities/StaticEntity.h b/Library/include/entities/StaticEntity.h
diff --git a/Library/include/entities/animation/BSTrajectory.h b/Library/include/entities/animation/BSTrajectory.h
diff --git a/Library/include/entities/animation/CRTrajectory.h b/Library/include/entities/animation/CRTrajectory.h
diff --git a/Library/include/entities/animation/ManualTrajectory.h b/Library/include/entities/animation/ManualTrajectory.h
diff --git a/Library/include/entities/animation/PWLTrajectory.h b/Library/include/entities/animation/PWLTrajectory.h
diff --git a/Library/include/entities/animation/Trajectory.h b/Library/include/entities/animation/Trajectory.h
diff --git a/Library/include/entities/forcefields/Atmosphere.h b/Library/include/entities/forcefields/Atmosphere.h
diff --git a/Library/include/entities/forcefields/Jet.h b/Library/include/entities/forcefields/Jet.h
diff --git a/Library/include/entities/forcefields/Ocean.h b/Library/include/entities/forcefields/Ocean.h
diff --git a/Library/include/entities/forcefields/Pipe.h b/Library/include/entities/forcefields/Pipe.h
diff --git a/Library/include/entities/forcefields/Stream.h b/Library/include/entities/forcefields/Stream.h
diff --git a/Library/include/entities/forcefields/Trigger.h b/Library/include/entities/forcefields/Trigger.h
diff --git a/Library/include/entities/forcefields/Uniform.h b/Library/include/entities/forcefields/Uniform.h
diff --git a/Library/include/entities/forcefields/VelocityField.h b/Library/include/entities/forcefields/VelocityField.h
diff --git a/Library/include/entities/solids/Box.h b/Library/include/entities/solids/Box.h
diff --git a/Library/include/entities/solids/Compound.h b/Library/include/entities/solids/Compound.h
diff --git a/Library/include/entities/solids/Cylinder.h b/Library/include/entities/solids/Cylinder.h
diff --git a/Library/include/entities/solids/Polyhedron.h b/Library/include/entities/solids/Polyhedron.h
diff --git a/Library/include/entities/solids/Sphere.h b/Library/include/entities/solids/Sphere.h
diff --git a/Library/include/entities/solids/Torus.h b/Library/include/entities/solids/Torus.h
diff --git a/Library/include/entities/solids/Wing.h b/Library/include/entities/solids/Wing.h
diff --git a/Library/include/entities/statics/Obstacle.h b/Library/include/entities/statics/Obstacle.h
diff --git a/Library/include/entities/statics/Plane.h b/Library/include/entities/statics/Plane.h
diff --git a/Library/include/entities/statics/Terrain.h b/Library/include/entities/statics/Terrain.h
diff --git a/Library/include/graphics/GLSLShader.h b/Library/include/graphics/GLSLShader.h
diff --git a/Library/include/graphics/IMGUI.h b/Library/include/graphics/IMGUI.h
diff --git a/Library/include/graphics/OpenGLAtmosphere.h b/Library/include/graphics/OpenGLAtmosphere.h
diff --git a/Library/include/graphics/OpenGLCamera.h b/Library/include/graphics/OpenGLCamera.h
diff --git a/Library/include/graphics/OpenGLConsole.h b/Library/include/graphics/OpenGLConsole.h
diff --git a/Library/include/graphics/OpenGLContent.h b/Library/include/graphics/OpenGLContent.h
diff --git a/Library/include/graphics/OpenGLDataStructs.h b/Library/include/graphics/OpenGLDataStructs.h
diff --git a/Library/include/graphics/OpenGLDebugDrawer.h b/Library/include/graphics/OpenGLDebugDrawer.h
diff --git a/Library/include/graphics/OpenGLDepthCamera.h b/Library/include/graphics/OpenGLDepthCamera.h
diff --git a/Library/include/graphics/OpenGLFLS.h b/Library/include/graphics/OpenGLFLS.h
diff --git a/Library/include/graphics/OpenGLFlatOcean.h b/Library/include/graphics/OpenGLFlatOcean.h
diff --git a/Library/include/graphics/OpenGLLight.h b/Library/include/graphics/OpenGLLight.h
diff --git a/Library/include/graphics/OpenGLMSIS.h b/Library/include/graphics/OpenGLMSIS.h
diff --git a/Library/include/graphics/OpenGLOcean.h b/Library/include/graphics/OpenGLOcean.h
diff --git a/Library/include/graphics/OpenGLOceanParticles.h b/Library/include/graphics/OpenGLOceanParticles.h
diff --git a/Library/include/graphics/OpenGLParticles.h b/Library/include/graphics/OpenGLParticles.h
diff --git a/Library/include/graphics/OpenGLPipeline.h b/Library/include/graphics/OpenGLPipeline.h
diff --git a/Library/include/graphics/OpenGLPointLight.h b/Library/include/graphics/OpenGLPointLight.h
diff --git a/Library/include/graphics/OpenGLPrinter.h b/Library/include/graphics/OpenGLPrinter.h
diff --git a/Library/include/graphics/OpenGLRealCamera.h b/Library/include/graphics/OpenGLRealCamera.h
diff --git a/Library/include/graphics/OpenGLRealOcean.h b/Library/include/graphics/OpenGLRealOcean.h
diff --git a/Library/include/graphics/OpenGLSSS.h b/Library/include/graphics/OpenGLSSS.h
diff --git a/Library/include/graphics/OpenGLSonar.h b/Library/include/graphics/OpenGLSonar.h
diff --git a/Library/include/graphics/OpenGLSpotLight.h b/Library/include/graphics/OpenGLSpotLight.h
diff --git a/Library/include/graphics/OpenGLState.h b/Library/include/graphics/OpenGLState.h
diff --git a/Library/include/graphics/OpenGLTrackball.h b/Library/include/graphics/OpenGLTrackball.h
diff --git a/Library/include/graphics/OpenGLView.h b/Library/include/graphics/OpenGLView.h
diff --git a/Library/include/joints/CylindricalJoint.h b/Library/include/joints/CylindricalJoint.h
diff --git a/Library/include/joints/FixedJoint.h b/Library/include/joints/FixedJoint.h
diff --git a/Library/include/joints/Joint.h b/Library/include/joints/Joint.h
diff --git a/Library/include/joints/PrismaticJoint.h b/Library/include/joints/PrismaticJoint.h
diff --git a/Library/include/joints/RevoluteJoint.h b/Library/include/joints/RevoluteJoint.h
diff --git a/Library/include/joints/SphericalJoint.h b/Library/include/joints/SphericalJoint.h
diff --git a/Library/include/joints/SpringJoint.h b/Library/include/joints/SpringJoint.h
diff --git a/Library/include/sensors/Contact.h b/Library/include/sensors/Contact.h
diff --git a/Library/include/sensors/Sample.h b/Library/include/sensors/Sample.h
diff --git a/Library/include/sensors/ScalarSensor.h b/Library/include/sensors/ScalarSensor.h
@@ -32,7 +32,7 @@
 namespace sf
 {
     //! An enum defining types of scalar sensors.
-    enum class ScalarSensorType {ACC, CURRENT, DVL, COMPASS, FT, GPS, GYRO, IMU, INS, MULTIBEAM, ODOM, PRESSURE, PROFILER, ENCODER, TORQUE, POSE};
+    enum class ScalarSensorType {ACC, CURRENT, DVL, COMPASS, FT, GPS, GYRO, IMU, INS, MULTIBEAM, ODOM, PRESSURE, PROFILER, ENCODER, TORQUE, POSE, LASERMEMS};
 
     //! An enum defining the type of quantity represented by the measurement.
     enum class QuantityType
@@ -105,7 +105,7 @@ namespace sf
         virtual void InternalUpdate(Scalar dt) = 0;
 
         //! A method returning the type of the sensor.
-        virtual SensorType getType() const = 0;
+        virtual SensorType getType() = 0;
 
         //! A method resetting the sensor.
         virtual void Reset();
@@ -161,7 +161,7 @@ namespace sf
         SensorChannel getSensorChannelDescription(unsigned int channel) const;
 
         //! A method returning the type of scalar sensor.
-        virtual ScalarSensorType getScalarSensorType() const = 0;
+        virtual ScalarSensorType getScalarSensorType() = 0;
 
         //! A method returning the sensor measurement frame.
         virtual Transform getSensorFrame() const = 0;