diff --git a/CMakeLists.txt b/CMakeLists.txt index cd3aead..f414c3d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,15 +1,89 @@ cmake_minimum_required(VERSION 3.8) project(simulation) + +# Find required packages find_package(ament_cmake REQUIRED) +find_package(ignition-plugin1 REQUIRED COMPONENTS register) +find_package(ignition-gazebo6 REQUIRED) +find_package(ignition-physics5 REQUIRED) +find_package(ignition-common4 REQUIRED) +find_package(ignition-rendering6 REQUIRED) + +# Set Gazebo plugin and sim versions +set(IGNITION_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) +set(IGNITION_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) +set(IGNITION_COMMON_VER ${ignition-common4_VERSION_MAJOR}) +set(IGNITION_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) +set(IGNITION_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) + +# Add the plugins directory to the include path +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/Plugins) + +# Add the SolarPanelPlugin library +add_library(SolarPanelPlugin SHARED + plugins/SolarPanelPlugin.cc +) + +# Link the SolarPanelPlugin library with required dependencies +target_link_libraries(SolarPanelPlugin + PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER} + PRIVATE ignition-common${IGNITION_COMMON_VER}::ignition-common${IGNITION_COMMON_VER} + PRIVATE ignition-rendering${IGNITION_RENDERING_VER}::ignition-rendering${IGNITION_RENDERING_VER} + PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core +) + +# Add the RadioisotopeThermalGeneratorPlugin library +add_library(RadioisotopeThermalGeneratorPlugin SHARED + plugins/RadioisotopeThermalGeneratorPlugin.cc +) + +# Link the RadioisotopeThermalGeneratorPlugin library with required dependencies +target_link_libraries(RadioisotopeThermalGeneratorPlugin + PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER} + PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core +) +# Add the RechargeableBatteryPlugin library +add_library(RechargeableBatteryPlugin SHARED + plugins/RechargeableBatteryPlugin.cc +) + +# Link the RechargeableBatteryPlugin library with required dependencies +target_link_libraries(RechargeableBatteryPlugin + PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER} + PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core +) + +# add the SensorPowerSystemPlugin library +add_library(SensorPowerSystemPlugin SHARED + plugins/SensorPowerSystemPlugin.cc +) + +# Link the SensorPowerSystemPlugin library with required dependencies +target_link_libraries(SensorPowerSystemPlugin + PRIVATE ignition-plugin${IGNITION_PLUGIN_VER}::ignition-plugin${IGNITION_PLUGIN_VER} + PRIVATE ignition-gazebo${IGNITION_SIM_VER}::core + PRIVATE ignition-gazebo${IGNITION_SIM_VER}::ignition-gazebo${IGNITION_SIM_VER} + PRIVATE ignition-sensors${IGNITION_SENSORS_VER}::ignition-sensors${IGNITION_SENSORS_VER} +) + +# Create the models directory file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/models/curiosity_path) +# Install directories install(DIRECTORY models ${CMAKE_CURRENT_BINARY_DIR}/models DESTINATION share/${PROJECT_NAME}/ ) +# Install the plugin library +install(TARGETS SolarPanelPlugin RadioisotopeThermalGeneratorPlugin RechargeableBatteryPlugin SensorPowerSystemPlugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") ament_package() diff --git a/models/lunar_pole_exploration_rover/meshes/body.dae b/models/lunar_pole_exploration_rover/meshes/body.dae new file mode 100644 index 0000000..f9dcfc4 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/body.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2f3a74df38acf8b987b2fe5d8f2ae8eb0469a91ac784904e31d4df15d5301fce +size 712393 diff --git a/models/lunar_pole_exploration_rover/meshes/chassis.dae b/models/lunar_pole_exploration_rover/meshes/chassis.dae new file mode 100644 index 0000000..fae6706 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/chassis.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d42666a5bed40344cae1dec6240505883ce57bee0cb751c4fe1b78e2fb006db6 +size 276723 diff --git a/models/lunar_pole_exploration_rover/meshes/chassis_texture.jpg b/models/lunar_pole_exploration_rover/meshes/chassis_texture.jpg new file mode 100644 index 0000000..c134d82 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/chassis_texture.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8fc4f66b6d59a62929897db7b2f26eba2d546c252d5095b17966b7db834b68f8 +size 1152405 diff --git a/models/lunar_pole_exploration_rover/meshes/mast_camera.dae b/models/lunar_pole_exploration_rover/meshes/mast_camera.dae new file mode 100644 index 0000000..f50290e --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/mast_camera.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ac8e6bee0fa8c3ce8940a81412a5cdf4531ee38a01c1388e7ba95842ce07871c +size 1446423 diff --git a/models/lunar_pole_exploration_rover/meshes/mast_head_pivot.dae b/models/lunar_pole_exploration_rover/meshes/mast_head_pivot.dae new file mode 100644 index 0000000..e8887d6 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/mast_head_pivot.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:39b9c0f2634f71570f881aa05537d3264d999ccfaf18f4672907df9ec0d861ef +size 376445 diff --git a/models/lunar_pole_exploration_rover/meshes/solar_panel.dae b/models/lunar_pole_exploration_rover/meshes/solar_panel.dae new file mode 100644 index 0000000..556f420 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/solar_panel.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:687f8db361f701f119864284e9a4f208848871bb6b396f7e3085a112af13f4eb +size 80499 diff --git a/models/lunar_pole_exploration_rover/meshes/suspension_left.dae b/models/lunar_pole_exploration_rover/meshes/suspension_left.dae new file mode 100644 index 0000000..c135419 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/suspension_left.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0af461bff5356db9bb5035d7361e3320de21749eea9badc57cfdf4f4098af036 +size 380242 diff --git a/models/lunar_pole_exploration_rover/meshes/suspension_right.dae b/models/lunar_pole_exploration_rover/meshes/suspension_right.dae new file mode 100644 index 0000000..e4031d4 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/suspension_right.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:037c3f078870be7144fe826c2202577b5723d38bd8ec7235026416291bff3441 +size 387452 diff --git a/models/lunar_pole_exploration_rover/meshes/wheel_axle_left.dae b/models/lunar_pole_exploration_rover/meshes/wheel_axle_left.dae new file mode 100644 index 0000000..7e65734 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/wheel_axle_left.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0a178c700c5591826f6cfcd47e5a3fab2c1bae7a3596b5a8f5a8491ba72dd8ef +size 633339 diff --git a/models/lunar_pole_exploration_rover/meshes/wheel_axle_right.dae b/models/lunar_pole_exploration_rover/meshes/wheel_axle_right.dae new file mode 100644 index 0000000..a3d6871 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/wheel_axle_right.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dc8c60965591d71d28c1f88703d01a8d557bc81b746dddefcca625c8ee3a8ebf +size 636939 diff --git a/models/lunar_pole_exploration_rover/meshes/wheel_left.dae b/models/lunar_pole_exploration_rover/meshes/wheel_left.dae new file mode 100644 index 0000000..8899d22 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/wheel_left.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8de571f166255ef404c156b40514fd1cd77cebce2b4032732b376d4e184e5711 +size 3640562 diff --git a/models/lunar_pole_exploration_rover/meshes/wheel_right.dae b/models/lunar_pole_exploration_rover/meshes/wheel_right.dae new file mode 100644 index 0000000..de9e7a3 --- /dev/null +++ b/models/lunar_pole_exploration_rover/meshes/wheel_right.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48951a23d91fe6b1d7e7d0c2ebd260a2a4583154af4b8b083ec9cf85a83d2258 +size 3699398 diff --git a/models/lunar_pole_exploration_rover/urdf/chassis.xacro b/models/lunar_pole_exploration_rover/urdf/chassis.xacro new file mode 100644 index 0000000..9a0453d --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/chassis.xacro @@ -0,0 +1,355 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + diff --git a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo new file mode 100644 index 0000000..4f81d8c --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.gazebo @@ -0,0 +1,490 @@ + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + robot_description + robot_state_publisher + $(find lunar_pole_exploration_rover)/config/lunar_pole_exploration_rover_control.yaml + + + + /odom + /base_footprint + 10 + + + + ogre2 + 0.0 0.0 0.0 1 + true + + + + left_solar_panel + 150.0 + + + right_solar_panel + 150.0 + + + rear_solar_panel + 150.0 + + + + + + + + + rechargeable_battery + 30.0 + 30.0 + 30.0 + -3.0 + 960.0 + 960.0 + 0.2 + 1.0 + + + left_solar_panel/solar_panel_output + right_solar_panel/solar_panel_output + rear_solar_panel/solar_panel_output + + + 100.0 + /battery/discharge + + + + + + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.22173 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + navcam_left/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.22173 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + navcam_right/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + aftcam_left/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + aftcam_right/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + hazcam_left_front/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + hazcam_left_rear/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + hazcam_right_front/image_raw + + + + + + 1 + 10.0 + 83.0 + rechargeable_battery + + 1.0 0 0 0 0 0 + 1.91986 + + 2048 + 2048 + L16 + + + 0.01 + 100 + + + gaussian + 0.007 + + + 1 + hazcam_right_rear/image_raw + + + + + + 1 + 100 + true + 16.0 + rechargeable_battery + imu + + + + + 0.0 + 0.0001 + + + + + 0.0 + 0.0001 + + + + + 0.0 + 0.0001 + + + + + + + 0.0 + 0.01 + + + + + 0.0 + 0.01 + + + + + 0.0 + 0.01 + + + + + + + + + 0.5 + 0.5 + + + + 0.0 + 0.0 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + 0.5 + 0.5 + + + + + 0.5 + 0.5 + + + + diff --git a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.xacro b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.xacro new file mode 100644 index 0000000..07aaa50 --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover_properties.xacro b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover_properties.xacro new file mode 100644 index 0000000..3b700d6 --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/lunar_pole_exploration_rover_properties.xacro @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/lunar_pole_exploration_rover/urdf/macros.xacro b/models/lunar_pole_exploration_rover/urdf/macros.xacro new file mode 100644 index 0000000..9f70611 --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/macros.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/models/lunar_pole_exploration_rover/urdf/mast_head.xacro b/models/lunar_pole_exploration_rover/urdf/mast_head.xacro new file mode 100644 index 0000000..a3a6840 --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/mast_head.xacro @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + \ No newline at end of file diff --git a/models/lunar_pole_exploration_rover/urdf/wheel.xacro b/models/lunar_pole_exploration_rover/urdf/wheel.xacro new file mode 100644 index 0000000..a4c5dd2 --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/wheel.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + 1.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + 100.0 + 0.001 + 1.0 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + \ No newline at end of file diff --git a/models/lunar_pole_exploration_rover/urdf/wheel_assembly.xacro b/models/lunar_pole_exploration_rover/urdf/wheel_assembly.xacro new file mode 100644 index 0000000..5dce32e --- /dev/null +++ b/models/lunar_pole_exploration_rover/urdf/wheel_assembly.xacro @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + \ No newline at end of file diff --git a/models/moon_mons_mouton/meshes/monsMouton4k.png b/models/moon_mons_mouton/meshes/monsMouton4k.png new file mode 100644 index 0000000..57916ca --- /dev/null +++ b/models/moon_mons_mouton/meshes/monsMouton4k.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24455ec452b820512e0bc4102659da6e3716067d0c3255e85cbfc1514d5c4acb +size 24372959 diff --git a/models/moon_mons_mouton/meshes/mons_mouton_textured_low.dae b/models/moon_mons_mouton/meshes/mons_mouton_textured_low.dae new file mode 100644 index 0000000..9a1cab5 --- /dev/null +++ b/models/moon_mons_mouton/meshes/mons_mouton_textured_low.dae @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f72d67f3a6368da72eda4b88f52ee78838c0f39083eb6b3259024cdd436c0884 +size 18534477 diff --git a/models/moon_mons_mouton/model.config b/models/moon_mons_mouton/model.config new file mode 100644 index 0000000..c134315 --- /dev/null +++ b/models/moon_mons_mouton/model.config @@ -0,0 +1,13 @@ + + + moon_mons_mouton + 0.1.0 + + Robin BARAN + r-baran@hotmail.fr + + model.sdf + + The Mons Mouton (also called Liebniz Beta) is located near the south pole of the moon and has been sighted as landing zone for assessing water ice deposits. This model was generated according to site20 by using DEM from the NASA LRO mission: https://pgda.gsfc.nasa.gov/products/78 (Barker, M.K., et al. (2021), Improved LOLA Elevation Maps for South Pole Landing Sites: Error Estimates and Their Impact on Illumination Conditions, Planetary & Space Science, Volume 203, 1 September 2021, 105119, doi:10.1016/j.pss.2020.105119.) + + diff --git a/models/moon_mons_mouton/model.sdf b/models/moon_mons_mouton/model.sdf new file mode 100644 index 0000000..69240ca --- /dev/null +++ b/models/moon_mons_mouton/model.sdf @@ -0,0 +1,64 @@ + + + + 1 + + + 0.25 + + 0.00015 + 0.000000 + 0.000000 + 0.00015 + 0.000000 + 0.00015 + + + + + + model://moon_mons_mouton/meshes/mons_mouton_textured_low.dae + 1 1 1 + + + + + + 30.0 + 30.0 + + + + + 1000000.0 + 100.0 + 1.0 + 0.002 + + + + + + true + + + model://moon_mons_mouton/meshes/mons_mouton_textured_low.dae + 1 1 1 + + + + + + 0.000000 + 0.000000 + + 0 + 0 + 1 + + + diff --git a/package.xml b/package.xml index 0d3db25..7e96866 100644 --- a/package.xml +++ b/package.xml @@ -9,9 +9,17 @@ ament_cmake + ign-cmake2 + ign-plugin1 + ign-common4 + ign-gazebo6 + ign-rendering6 + ign-sensors6 + xacro ament_cmake + diff --git a/plugins/RadioisotopeThermalGeneratorPlugin.cc b/plugins/RadioisotopeThermalGeneratorPlugin.cc new file mode 100644 index 0000000..4e7d056 --- /dev/null +++ b/plugins/RadioisotopeThermalGeneratorPlugin.cc @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2024 Robin Baran + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "RadioisotopeThermalGeneratorPlugin.hh" + +#include +#include "ignition/gazebo/Model.hh" + +#include + +#include + +#include + +using namespace simulation; + +/// \brief Private data class for RadioisotopeThermalGeneratorPlugin +class simulation::RadioisotopeThermalGeneratorPluginPrivate +{ + /// \brief Name of the link +public: + std::string linkName; + + /// \brief Name of the model +public: + std::string modelName; + + /// \brief Name of the topic where the data will be published +public: + std::string topicName; + + /// \brief Power output of the radioisotope thermal generator +public: + double nominalPower; + + /// \brief Ignition communication node +public: + ignition::transport::Node node; + + /// \brief Publisher for the radioisotope thermal generator output +public: + ignition::transport::Node::Publisher nominalPowerPub; +}; + +////////////////////////////////////////////////// +RadioisotopeThermalGeneratorPlugin::RadioisotopeThermalGeneratorPlugin() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +RadioisotopeThermalGeneratorPlugin::~RadioisotopeThermalGeneratorPlugin() = default; + +////////////////////////////////////////////////// +void RadioisotopeThermalGeneratorPlugin::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + // Store the pointer to the model the RTG is under + auto model = ignition::gazebo::Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "Radioisotope Thermal Generator plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->modelName = model.Name(_ecm); + + // Load params + if (_sdf->HasElement("link_name")) + { + this->dataPtr->linkName = _sdf->Get("link_name"); + this->dataPtr->topicName = "/model/" + this->dataPtr->modelName + "/" + this->dataPtr->linkName + "/radioisotope_thermal_generator_output"; + auto validTopic = ignition::transport::TopicUtils::AsValidTopic(this->dataPtr->topicName); + if (validTopic.empty()) + { + ignerr << "Failed to create valid topic [" << this->dataPtr->topicName << "]" << std::endl; + return; + } + // Advertise topic where data will be published + this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise(validTopic); + } + else + { + ignerr << "Radioisotope Thermal Generator plugin should have a element. " + << "Failed to initialize." << std::endl; + return; + } + + if (_sdf->HasElement("nominal_power")) + { + this->dataPtr->nominalPower = _sdf->Get("nominal_power"); + } + else + { + ignerr << "Radioisotope Thermal Generator plugin should have a element. " + << "Failed to initialize." << std::endl; + return; + } +} + +////////////////////////////////////////////////// +void RadioisotopeThermalGeneratorPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("RadioisotopeThermalGeneratorPlugin::PostUpdate"); + if (_info.paused) + { + return; + } + // Publish result + ignition::msgs::Float msg; + msg.set_data(this->dataPtr->nominalPower); + this->dataPtr->nominalPowerPub.Publish(msg); + igndbg << "Published RTG output: " << this->dataPtr->nominalPower << std::endl; +} + +IGNITION_ADD_PLUGIN(RadioisotopeThermalGeneratorPlugin, ignition::gazebo::System, + RadioisotopeThermalGeneratorPlugin::ISystemConfigure, + RadioisotopeThermalGeneratorPlugin::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(RadioisotopeThermalGeneratorPlugin, "simulation::RadioisotopeThermalGeneratorPlugin") diff --git a/plugins/RadioisotopeThermalGeneratorPlugin.hh b/plugins/RadioisotopeThermalGeneratorPlugin.hh new file mode 100644 index 0000000..dd95777 --- /dev/null +++ b/plugins/RadioisotopeThermalGeneratorPlugin.hh @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2024 Robin Baran + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef Radioisotope_Thermal_Generator_Plugin_HH_ +#define Radioisotope_Thermal_Generator_Plugin_HH_ + +#include +#include + +namespace simulation +{ + + // Forward declaration + class RadioisotopeThermalGeneratorPluginPrivate; + + /// \class RadioisotopeThermalGenerator RadioisotopeThermalGenerator.hh + /// \brief Radioisotope Thermal Generator plugin. plugin allows to simulate an radioisotope thermal generator power output. It provides a constant power supply. + + /// ## System Parameters + /// + /// - `link_name`: The name of the link where the radioisotope thermal generator is attached. + /// - `nominal_power`: The nominal power output of the radioisotope thermal generator. + class RadioisotopeThermalGeneratorPlugin : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPostUpdate + { + + /// \brief Constructor + public: + RadioisotopeThermalGeneratorPlugin(); + + /// \brief Destructor + public: + ~RadioisotopeThermalGeneratorPlugin() override; + + /// Documentation inherited + public: + void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + /// Documentation inherited + public: + void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) final; + + /// \brief Private data pointer + private: + std::unique_ptr dataPtr; + }; +} +#endif // Radioisotope_Thermal_Generator_Plugin_HH_ diff --git a/plugins/RechargeableBatteryPlugin.cc b/plugins/RechargeableBatteryPlugin.cc new file mode 100644 index 0000000..92b03e8 --- /dev/null +++ b/plugins/RechargeableBatteryPlugin.cc @@ -0,0 +1,779 @@ +/* + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2024 Robin Baran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "RechargeableBatteryPlugin.hh" + +#include +#include +#include "ignition/gazebo/components/BatterySoC.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include "gz/sim/Model.hh" +#include + +using namespace simulation; + +/// \brief Power Source Information +struct powerSourceInfo +{ + /// \brief Power source id + int id{0}; + + /// \brief Power source power + double nominalPower{0.0}; + + /// \brief Flag to check if the power source has been updated + bool dataUpdated{false}; + + /// \brief Mutex to protect the power source state + std::unique_ptr mutex_ptr = std::make_unique(); +}; + +class simulation::RechargeableBatteryPluginPrivate +{ + + /// \brief Reset the plugin +public: + void Reset(); + /// \brief Name that identifies a battery. + + /// \brief Get the current state of charge of the battery. + /// \return State of charge of the battery in range [0.0, 1.0]. +public: + double StateOfCharge() const; + + /// \brief Callback connected to additional topics that can start battery + /// draining. + /// \param[in] _data Message data. + /// \param[in] _size Message data size. + /// \param[in] _info Information about the message. +public: + void OnBatteryDrainingMsg( + const char *_data, const size_t _size, + const ignition::transport::MessageInfo &_info); + + /// \brief Callback connected to additional topics that can stop battery + /// draining. + /// \param[in] _data Message data. + /// \param[in] _size Message data size. + /// \param[in] _info Information about the message. +public: + void OnBatteryStopDrainingMsg( + const char *_data, const size_t _size, + const ignition::transport::MessageInfo &_info); + + /// \brief Callback connected to power source topics. + /// \param[in] _id The id of the power source. + /// \param[in] _msg The message containing the power source power. +public: + void OnPowerSourceMsg(int _id, + const ignition::msgs::Float &_msg); + + /// \brief Ignition communication node +public: + ignition::transport::Node node; + + /// \brief Battery state of charge message publisher +public: + ignition::transport::Node::Publisher statePub; + + /// \brief Total power supply publisher +public: + ignition::transport::Node::Publisher totalPowerSupplyPub; + + /// \brief Total power consumption publisher +public: + ignition::transport::Node::Publisher totalPowerConsumptionPub; + + /// \brief Battery consumer identifier. + /// Current implementation limits one consumer (Model) per battery. +public: + int32_t consumerId; + + /// \brief Name that identifies a battery. +public: + std::string batteryName; + + /// \brief Pointer to battery contained in link. +public: + gz::common::BatteryPtr battery; + + /// \brief Whether warning that battery has drained has been printed once. +public: + bool drainPrinted{false}; + + /// \brief Battery entity +public: + ignition::gazebo::Entity batteryEntity{ignition::gazebo::kNullEntity}; + + /// \brief modelName +public: + std::string modelName; + + /// \brief Model entity +public: + gz::sim::Model model{ignition::gazebo::kNullEntity}; + + /// \brief Open-circuit voltage. + /// E(t) = e0 + e1 * Q(t) / c +public: + double e0{0.0}; + +public: + double e1{0.0}; + + /// \brief Initial battery charge in Ah. +public: + double q0{0.0}; + + /// \brief Battery capacity in Ah. +public: + double c{0.0}; + + /// \brief Battery inner resistance in Ohm. +public: + double r{0.0}; + + /// \brief Current low-pass filter characteristic time in seconds [0, 1]. +public: + double tau{1.0}; + + /// \brief Raw battery current in A. +public: + double iraw{0.0}; + + /// \brief Smoothed battery current in A. +public: + double ismooth{0.0}; + + /// \brief Instantaneous battery charge in Ah. +public: + double q{0.0}; + + /// \brief State of charge [0, 1]. +public: + double soc{1.0}; + + /// \brief Simulation time handled during a single update. +public: + std::chrono::steady_clock::duration stepSize; + + /// \brief Flag on whether the battery should start draining +public: + bool startDraining = false; + + /// \brief The start time when battery starts draining in seconds +public: + int drainStartTime = -1; + + /// \brief Book keep the last time printed, so as to not pollute dbg messages + /// in minutes +public: + int lastPrintTime = -1; + + /// \brief Initial power load set trough config +public: + double initialPowerLoad = 0.0; + + /// \brief Total power supply +public: + double totalPowerSupply = 0.0; + + /// \brief Flag to check if the battery is charging +public: + bool isCharging = false; + + /// \brief Vector of power sources information +public: + std::vector powerSourcesInfo; +}; + +///////////////////////////////////////////////// +RechargeableBatteryPlugin::RechargeableBatteryPlugin() + : System(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +RechargeableBatteryPlugin::~RechargeableBatteryPlugin() +{ + this->dataPtr->Reset(); + + if (this->dataPtr->battery) + { + // consumer-specific + if (this->dataPtr->consumerId != -1) + { + this->dataPtr->battery->RemoveConsumer(this->dataPtr->consumerId); + } + + // This is needed so that common::Battery stops calling the update function + // of this object, when this object is destroyed. Else seg fault in test, + // though no seg fault in actual run. + this->dataPtr->battery->ResetUpdateFunc(); + } +} + +///////////////////////////////////////////////// +void RechargeableBatteryPlugin::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + // Store the pointer to the model this battery is under + auto model = ignition::gazebo::Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "Battery plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + if (_sdf->HasElement("open_circuit_voltage_constant_coef")) + this->dataPtr->e0 = _sdf->Get("open_circuit_voltage_constant_coef"); + + if (_sdf->HasElement("open_circuit_voltage_linear_coef")) + this->dataPtr->e1 = _sdf->Get("open_circuit_voltage_linear_coef"); + + if (_sdf->HasElement("capacity")) + this->dataPtr->c = _sdf->Get("capacity"); + + if (this->dataPtr->c <= 0) + { + ignerr << "No or incorrect value specified. Capacity should be " + << "greater than 0.\n"; + return; + } + + this->dataPtr->q0 = this->dataPtr->c; + if (_sdf->HasElement("initial_charge")) + { + this->dataPtr->q0 = _sdf->Get("initial_charge"); + if (this->dataPtr->q0 > this->dataPtr->c || this->dataPtr->q0 < 0) + { + ignerr << " value should be between [0, ]." + << std::endl; + this->dataPtr->q0 = + std::max(0.0, std::min(this->dataPtr->q0, this->dataPtr->c)); + ignerr << "Setting to [" << this->dataPtr->q0 + << "] instead." << std::endl; + } + } + + this->dataPtr->q = this->dataPtr->q0; + + if (_sdf->HasElement("resistance")) + this->dataPtr->r = _sdf->Get("resistance"); + + if (_sdf->HasElement("smooth_current_tau")) + { + this->dataPtr->tau = _sdf->Get("smooth_current_tau"); + if (this->dataPtr->tau <= 0) + { + ignerr << " value should be positive. " + << "Using [1] instead." << std::endl; + this->dataPtr->tau = 1; + } + } + + if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) + { + this->dataPtr->batteryName = _sdf->Get("battery_name"); + auto initVoltage = _sdf->Get("voltage"); + igndbg << "Battery name: " << this->dataPtr->batteryName << std::endl; + igndbg << "Initial voltage: " << initVoltage << std::endl; + + // Create battery entity and some components + this->dataPtr->batteryEntity = _ecm.CreateEntity(); + _ecm.CreateComponent(this->dataPtr->batteryEntity, ignition::gazebo::components::Name( + this->dataPtr->batteryName)); + _ecm.SetParentEntity(this->dataPtr->batteryEntity, _entity); + + // Create actual battery and assign update function + this->dataPtr->battery = std::make_shared( + this->dataPtr->batteryName, initVoltage); + this->dataPtr->battery->Init(); + // print battery voltage + igndbg << "Battery voltage: " << this->dataPtr->battery->Voltage() << std::endl; + this->dataPtr->battery->SetUpdateFunc( + std::bind(&RechargeableBatteryPlugin::OnUpdateVoltage, this, + std::placeholders::_1)); + } + else + { + ignerr << "No or specified. Both are required.\n"; + return; + } + + // Consumer-specific + if (_sdf->HasElement("power_load")) + { + this->dataPtr->initialPowerLoad = _sdf->Get("power_load"); + this->dataPtr->consumerId = this->dataPtr->battery->AddConsumer(); + bool success = this->dataPtr->battery->SetPowerLoad( + this->dataPtr->consumerId, this->dataPtr->initialPowerLoad); + if (!success) + ignerr << "Failed to set consumer power load." << std::endl; + } + else + { + ignwarn << "Required attribute power_load missing " + << "in BatteryPlugin SDF" << std::endl; + } + if (_sdf->HasElement("start_draining")) + this->dataPtr->startDraining = _sdf->Get("start_draining"); + + // Subscribe to power draining topics, if any. + if (_sdf->HasElement("power_draining_topic")) + { + sdf::ElementConstPtr sdfElem = _sdf->FindElement("power_draining_topic"); + while (sdfElem) + { + const auto &topic = sdfElem->Get(); + this->dataPtr->node.SubscribeRaw(topic, + std::bind(&RechargeableBatteryPluginPrivate::OnBatteryDrainingMsg, + this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + ignmsg << "RechargeableBatteryPlugin subscribes to power draining topic [" + << topic << "]." << std::endl; + sdfElem = sdfElem->GetNextElement("power_draining_topic"); + } + } + + // Subscribe to stop power draining topics, if any. + if (_sdf->HasElement("stop_power_draining_topic")) + { + sdf::ElementConstPtr sdfElem = + _sdf->FindElement("stop_power_draining_topic"); + while (sdfElem) + { + const auto &topic = sdfElem->Get(); + this->dataPtr->node.SubscribeRaw(topic, + std::bind(&RechargeableBatteryPluginPrivate::OnBatteryStopDrainingMsg, + this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + ignmsg << "RechargeableBatteryPlugin subscribes to stop power draining topic [" + << topic << "]." << std::endl; + sdfElem = sdfElem->GetNextElement("power_draining_topic"); + } + } + + // subscriber to all power sources topics + if (_sdf->HasElement("power_source")) + { + sdf::ElementConstPtr powerSourceElem = _sdf->FindElement("power_source"); + int id = 0; + while (powerSourceElem) + { + const auto &topic = powerSourceElem->Get(); + std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/" + + topic}; + auto validPowerSourceTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + if (validPowerSourceTopic.empty()) + { + ignerr << "Failed to create valid topic. Not valid: [" + << topic << "]" << std::endl; + return; + } + powerSourceInfo powerSource; + powerSource.id = id; + powerSource.nominalPower = 0.0; + powerSource.dataUpdated = false; + std::function callback = std::bind(&RechargeableBatteryPluginPrivate::OnPowerSourceMsg, + this->dataPtr.get(), id, std::placeholders::_1); + this->dataPtr->node.Subscribe(validPowerSourceTopic, callback); + ignmsg << "RechargeableBatteryPlugin subscribes to power source topic [" + << validPowerSourceTopic << "]." << std::endl; + this->dataPtr->powerSourcesInfo.emplace_back(std::move(powerSource)); + powerSourceElem = powerSourceElem->GetNextElement("power_source"); + id++; + } + } + else + { + ignerr << "No power source topic specified." << std::endl; + } + + ignmsg << "RechargeableBatteryPlugin configured. Battery name: " + << this->dataPtr->battery->Name() << std::endl; + igndbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage() + << std::endl; + + this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; + // Initialize battery with initial calculated state of charge + _ecm.CreateComponent(this->dataPtr->batteryEntity, + ignition::gazebo::components::BatterySoC(this->dataPtr->soc)); + + // Setup battery state topic + std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/battery/" + this->dataPtr->battery->Name() + "/state"}; + + auto validStateTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + if (validStateTopic.empty()) + { + ignerr << "Failed to create valid state topic [" + << stateTopic << "]" << std::endl; + return; + } + + ignition::transport::AdvertiseMessageOptions opts; + opts.SetMsgsPerSec(50); + this->dataPtr->statePub = this->dataPtr->node.Advertise( + validStateTopic, opts); + + // Setup total power supply topic + std::string totalPowerSupplyTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/battery/" + this->dataPtr->battery->Name() + "/total_power_supply"}; + + auto validTotalPowerSupplyTopic = ignition::transport::TopicUtils::AsValidTopic(totalPowerSupplyTopic); + if (validTotalPowerSupplyTopic.empty()) + { + ignerr << "Failed to create valid total power supply topic [" + << totalPowerSupplyTopic << "]" << std::endl; + return; + } + + this->dataPtr->totalPowerSupplyPub = this->dataPtr->node.Advertise( + validTotalPowerSupplyTopic); + + // Setup total power consumption topic + std::string totalPowerConsumptionTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/battery/" + this->dataPtr->battery->Name() + "/total_power_consumption"}; + + auto validTotalPowerConsumptionTopic = ignition::transport::TopicUtils::AsValidTopic(totalPowerConsumptionTopic); + + if (validTotalPowerConsumptionTopic.empty()) + { + ignerr << "Failed to create valid total power consumption topic [" + << totalPowerConsumptionTopic << "]" << std::endl; + return; + } + + this->dataPtr->totalPowerConsumptionPub = this->dataPtr->node.Advertise( + validTotalPowerConsumptionTopic); +} + +///////////////////////////////////////////////// +void RechargeableBatteryPlugin::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("RechargeableBatteryPlugin::PreUpdate"); + // Recalculate total power load among all consumers + double totalPowerLoad = this->dataPtr->initialPowerLoad; + _ecm.Each( + [&](const ignition::gazebo::Entity & /*_entity*/, + const ignition::gazebo::components::BatteryPowerLoad *_batteryPowerLoadInfo) -> bool + { + if (_batteryPowerLoadInfo->Data().batteryId == + this->dataPtr->batteryEntity) + { + totalPowerLoad = totalPowerLoad + + _batteryPowerLoadInfo->Data().batteryPowerLoad; + } + return true; + }); + + // Publish total power consumption + ignition::msgs::Float totalPowerConsumptionMsg; + totalPowerConsumptionMsg.set_data(totalPowerLoad); + this->dataPtr->totalPowerConsumptionPub.Publish(totalPowerConsumptionMsg); + + bool success = this->dataPtr->battery->SetPowerLoad( + this->dataPtr->consumerId, totalPowerLoad); + if (!success) + ignerr << "Failed to set consumer power load." << std::endl; + + // start draining the battery if the robot has started moving + if (!this->dataPtr->startDraining) + { + const std::vector &joints = + _ecm.ChildrenByComponents(this->dataPtr->model.Entity(), ignition::gazebo::components::Joint()); + + for (ignition::gazebo::Entity jointEntity : joints) + + { + const auto *jointVelocityCmd = + _ecm.Component(jointEntity); + if (jointVelocityCmd) + { + for (double jointVel : jointVelocityCmd->Data()) + { + if (fabs(static_cast(jointVel)) > 0.01) + { + this->dataPtr->startDraining = true; + break; + } + } + } + + const auto *jointForceCmd = + _ecm.Component(jointEntity); + if (jointForceCmd) + { + for (double jointForce : jointForceCmd->Data()) + { + if (fabs(static_cast(jointForce)) > 0.01) + { + this->dataPtr->startDraining = true; + break; + } + } + } + } + } +} + +/////////////////////////////////////////////// +void RechargeableBatteryPlugin::Update(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("RechargeableBatteryPlugin::Update"); + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + if (_info.paused) + return; + + if (!this->dataPtr->startDraining) + return; + + // Find the time at which battery starts to drain + int simTime = static_cast( + std::chrono::duration_cast(_info.simTime).count()); + if (this->dataPtr->drainStartTime == -1) + this->dataPtr->drainStartTime = simTime; + + // Print drain time in minutes + int drainTime = (simTime - this->dataPtr->drainStartTime) / 60; + if (drainTime != this->dataPtr->lastPrintTime) + { + this->dataPtr->lastPrintTime = drainTime; + igndbg << "[Battery Plugin] Battery drain: " << drainTime << " minutes passed.\n"; + } + + // update step size + this->dataPtr->stepSize = _info.dt; + + // Sanity check: tau should be in the range [dt, +inf) + double dt = (std::chrono::duration_cast( + this->dataPtr->stepSize) + .count()) * + 1e-9; + if (this->dataPtr->tau < dt) + { + ignerr << " should be in the range [dt, +inf) but is " + << "configured with [" << this->dataPtr->tau << "]. We'll be using " + << "[" << dt << "] instead" << std::endl; + this->dataPtr->tau = dt; + } + + if (this->dataPtr->battery) + { + // Update battery component + this->dataPtr->battery->Update(); + auto *batteryComp = _ecm.Component( + this->dataPtr->batteryEntity); + + batteryComp->Data() = this->dataPtr->StateOfCharge(); + } +} + +///////////////////////////////////////////////// +void RechargeableBatteryPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("RechargeableBatteryPlugin::PostUpdate"); + // Nothing left to do if paused or the publisher wasn't created. + if (_info.paused || !this->dataPtr->statePub) + return; + + // Publish battery state + ignition::msgs::BatteryState msg; + msg.mutable_header()->mutable_stamp()->CopyFrom(ignition::gazebo::convert(_info.simTime)); + msg.set_voltage(this->dataPtr->battery->Voltage()); + msg.set_current(this->dataPtr->ismooth); + msg.set_charge(this->dataPtr->q); + msg.set_capacity(this->dataPtr->c); + msg.set_percentage(this->dataPtr->soc * 100); + + if (this->dataPtr->isCharging) + msg.set_power_supply_status(ignition::msgs::BatteryState::CHARGING); + else if (this->dataPtr->startDraining) + msg.set_power_supply_status(ignition::msgs::BatteryState::DISCHARGING); + else if (!this->dataPtr->StateOfCharge() > 0.9) + msg.set_power_supply_status(ignition::msgs::BatteryState::FULL); + else + msg.set_power_supply_status(ignition::msgs::BatteryState::NOT_CHARGING); + + this->dataPtr->statePub.Publish(msg); +} + +///////////////////////////////////////////////// +double RechargeableBatteryPlugin::OnUpdateVoltage(const ignition::common::Battery *_battery) +{ + IGN_ASSERT(_battery != nullptr, "Battery pointer is null"); + + if (fabs(_battery->Voltage()) < 1e-3) + return 0.0; + if (this->dataPtr->StateOfCharge() <= 0) + return _battery->Voltage(); + + auto prevSocInt = static_cast(this->dataPtr->StateOfCharge() * 100); + + // seconds + double dt = (std::chrono::duration_cast( + this->dataPtr->stepSize) + .count()) * + 1e-9; + double totalpower = 0.0; + double k = dt / this->dataPtr->tau; + + if (this->dataPtr->startDraining) + { + for (auto powerLoad : _battery->PowerLoads()) + totalpower += powerLoad.second; + } + + this->dataPtr->iraw = totalpower / _battery->Voltage(); + + // Update total power supply + for (auto &powerSource : this->dataPtr->powerSourcesInfo) + { + std::lock_guard lock(*(powerSource.mutex_ptr)); + if (powerSource.dataUpdated) + { + powerSource.dataUpdated = false; + this->dataPtr->totalPowerSupply += powerSource.nominalPower; + } + } + + // Publish total power supply + ignition::msgs::Float totalPowerSupplyMsg; + totalPowerSupplyMsg.set_data(this->dataPtr->totalPowerSupply); + this->dataPtr->totalPowerSupplyPub.Publish(totalPowerSupplyMsg); + + // check if the total powersupply is not zero + this->dataPtr->isCharging = this->dataPtr->totalPowerSupply > 1e-9 && this->dataPtr->StateOfCharge() < 0.9; + + // compute current due to power sources + float powerSourceCurrent = 0.0; + if (this->dataPtr->isCharging) + { + powerSourceCurrent = this->dataPtr->totalPowerSupply / _battery->Voltage(); + this->dataPtr->iraw -= powerSourceCurrent; + } + // reset total power supply to zero + this->dataPtr->totalPowerSupply = 0.0; + + this->dataPtr->ismooth = this->dataPtr->ismooth + k * + (this->dataPtr->iraw - this->dataPtr->ismooth); + + // Convert dt to hours + this->dataPtr->q = this->dataPtr->q - ((dt * this->dataPtr->ismooth) / + 3600.0); + + // open circuit voltage + double voltage = this->dataPtr->e0 + this->dataPtr->e1 * (1 - this->dataPtr->q / this->dataPtr->c) - this->dataPtr->r * this->dataPtr->ismooth; + + // Estimate state of charge + this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; + + // Throttle debug messages + auto socInt = static_cast(this->dataPtr->StateOfCharge() * 100); + + if (socInt % 10 == 0 && socInt != prevSocInt) + { + igndbg << "Battery: " << this->dataPtr->battery->Name() << std::endl; + igndbg << "PowerLoads().size(): " << _battery->PowerLoads().size() + << std::endl; + igndbg << "charging current: " << powerSourceCurrent << std::endl; + igndbg << "voltage: " << voltage << std::endl; + igndbg << "state of charge: " << this->dataPtr->StateOfCharge() + << " (q " << this->dataPtr->q << ")" << std::endl + << std::endl; + } + if (this->dataPtr->StateOfCharge() < 0 && !this->dataPtr->drainPrinted) + { + ignwarn << "Model " << this->dataPtr->modelName << " out of battery.\n"; + this->dataPtr->drainPrinted = true; + } + + return voltage; +} + +///////////////////////////////////////////////// +void RechargeableBatteryPluginPrivate::Reset() +{ + this->totalPowerSupply = 0.0; + this->ismooth = 0.0; + this->iraw = 0.0; + this->q = this->q0; + this->startDraining = false; +} + +////////////////////////////////////////////////// +void RechargeableBatteryPluginPrivate::OnBatteryDrainingMsg( + const char *, const size_t, const ignition::transport::MessageInfo &) +{ + this->startDraining = true; +} + +////////////////////////////////////////////////// +void RechargeableBatteryPluginPrivate::OnBatteryStopDrainingMsg( + const char *, const size_t, const ignition::transport::MessageInfo &) +{ + this->startDraining = false; +} + +////////////////////////////////////////////////// +void RechargeableBatteryPluginPrivate::OnPowerSourceMsg(int _id, + const ignition::msgs::Float &_msg) +{ + std::lock_guard lock(*(this->powerSourcesInfo[_id].mutex_ptr)); + this->powerSourcesInfo[_id].nominalPower = _msg.data(); + this->powerSourcesInfo[_id].dataUpdated = true; +} + +////////////////////////////////////////////////// +double RechargeableBatteryPluginPrivate::StateOfCharge() const +{ + return this->soc; +} + +#include + +IGNITION_ADD_PLUGIN(RechargeableBatteryPlugin, + ignition::gazebo::System, + RechargeableBatteryPlugin::ISystemConfigure, + RechargeableBatteryPlugin::ISystemPreUpdate, + RechargeableBatteryPlugin::ISystemUpdate, + RechargeableBatteryPlugin::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(RechargeableBatteryPlugin, "simulation::RechargeableBatteryPlugin") diff --git a/plugins/RechargeableBatteryPlugin.hh b/plugins/RechargeableBatteryPlugin.hh new file mode 100644 index 0000000..9d0aab1 --- /dev/null +++ b/plugins/RechargeableBatteryPlugin.hh @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2024 Robin Baran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RECHARGEABLE_BATTERY_PLUGIN_HH_ +#define RECHARGEABLE_BATTERY_PLUGIN_HH_ + +#include +#include +#include + + +namespace simulation +{ + // Forward declaration + class RechargeableBatteryPluginPrivate; + + /// \brief A plugin to simulate rechargeable batteries usage. + /// This was adapted from the LinearBatteryPlugin from Ignition Gazebo. + /// + /// This system processes the following sdf parameters: + /// - ``: The name of the battery(required) + /// - ``: The initial voltage of the battery(required) + /// - ``: Voltage at full charge + /// - ``: Amount of voltage drop when no charge + /// - ``: The initial charge of the battery (Ah) + /// - ``: Total charge that the battery can hold (Ah) + /// - ``: The internal resistance of the battery (Ohm) + /// - ``: Coefficient for smoothing the current [0, 1] + /// - ``: The power load on the battery (Watts) + /// - ``: Whether to start draining the battery + /// - ``: This is to start draining the battery + /// - ``: This is to stop draining the battery + /// - ``: This is to subscribe to power sources topics. Repeat as many times as needed with the same name + + class RechargeableBatteryPlugin + : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemUpdate, + public ignition::gazebo::ISystemPostUpdate + { + /// \brief Constructor + public: + RechargeableBatteryPlugin(); + + /// \brief Destructor + public: + ~RechargeableBatteryPlugin() override; + + /// Documentation Inherited + public: + void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) final; + + /// Documentation Inherited + public: + void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation Inherited + public: + void Update(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// Documentation Inherited + public: + void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Callback for Battery Update events. + /// \param[in] _battery Pointer to the battery that is to be updated. + /// \return The new voltage. + private: + double OnUpdateVoltage(const ignition::common::Battery *_battery); + + /// \brief Private data pointer + private: + std::unique_ptr dataPtr; + }; + +} +#endif // RECHARGEABLE_BATTERY_PLUGIN_HH_ diff --git a/plugins/SensorPowerSystemPlugin.cc b/plugins/SensorPowerSystemPlugin.cc new file mode 100644 index 0000000..ac3eee8 --- /dev/null +++ b/plugins/SensorPowerSystemPlugin.cc @@ -0,0 +1,349 @@ +/* + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2024 Robin Baran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "SensorPowerSystemPlugin.hh" +#include +#include +#include +#include +#include "gz/sim/components/BatterySoC.hh" +#include "gz/sim/components/BatteryPowerLoad.hh" +#include +#include +#include +#include "gz/sim/Model.hh" +#include +#include +#include "ignition/sensors/Sensor.hh" + +using namespace simulation; + +/// \brief Sensor information +struct SensorInfo +{ + /// \brief Sensor id + int id{0}; + + /// \brief Sensor name + std::string name{""}; + + /// \brief Keep track of the sensor state + bool enableSensor{false}; + + /// \brief Power load of the sensor + double powerLoad{0.0}; + + /// \brief Battery name + std::string batteryName{""}; + + /// \brief Battery entity + ignition::gazebo::Entity batteryEntity{ignition::gazebo::kNullEntity}; + + /// \brief Battery consumer entity + ignition::gazebo::Entity batteryConsumerEntity{ignition::gazebo::kNullEntity}; + + /// \brief Flag to check if the battery exists + bool batteryExist{false}; + + /// \brief Flag to check if it has enough battery + bool hasEnoughBattery{true}; + + /// \brief Mutex to protect the sensor state + std::unique_ptr mutex_ptr = std::make_unique(); + + /// \brief Flag to check if the sensor has been updated + bool dataUpdated{false}; +}; + +/// \brief Definition of the private data class +class simulation::SensorPowerSystemPrivate +{ + /// \brief Callback executed when a sensor is activated + /// \param[in] _id The id of the sensor + /// \param[in] _msg The message containing the activation state +public: + void OnActivateSensor(int _id, const ignition::msgs::Boolean &_msg); + + /// \brief Check if the battery has sufficient charge + /// \param[in] _ecm The entity component manager + /// \return True if the battery has sufficient charge +public: + void HasSufficientBattery(const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Model name +public: + std::string modelName; + + /// \brief Model entity +public: + gz::sim::Model model{ignition::gazebo::kNullEntity}; + + /// \brief Ignition communication node +public: + ignition::transport::Node node; + + /// \brief Sensors information +public: + std::vector sensorsInfo; + + /// \brief Flag to check if the battery is initialized +public: + bool batteriesInitialized{false}; +}; + +///////////////////////////////////////////////// +SensorPowerSystemPlugin::SensorPowerSystemPlugin() + : System(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +SensorPowerSystemPlugin::~SensorPowerSystemPlugin() = default; + +///////////////////////////////////////////////// +void SensorPowerSystemPlugin::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + // Store the pointer to the model this battery is under + auto model = ignition::gazebo::Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "SensorPowerSystemPlugin plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + // Read power load and battery name from the sensors + + // camera sensors + int sensorCount = 0; + + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Camera *_camera) -> bool + { + auto cameraName = _ecm.Component(_entity); + if (cameraName) + { + auto cameraPtr = _ecm.Component(_entity); + auto camera = cameraPtr->Data().CameraSensor(); + auto parent = camera->Element()->GetParent(); + if (parent->HasElement("power_load") && parent->HasElement("battery_name")) + { + SensorInfo sensorInfo; + sensorInfo.id = sensorCount; + sensorInfo.name = cameraName->Data(); + sensorInfo.powerLoad = parent->Get("power_load"); + sensorInfo.batteryName = parent->Get("battery_name"); + sensorInfo.enableSensor = true; + sensorInfo.dataUpdated = false; + igndbg << "CAMERA: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; + igndbg << "CAMERA: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; + igndbg << "CAMERA: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; + igndbg << "CAMERA: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; + igndbg << "CAMERA: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; + igndbg << "CAMERA id: " << sensorInfo.id << std::endl; + this->dataPtr->sensorsInfo.emplace_back(std::move(sensorInfo)); + sensorCount++; + } + } + return true; + }); + + // imu sensors + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Imu *_imu) -> bool + { + // get the imu name + auto imuName = _ecm.Component(_entity); + if (imuName) + { + auto imuPtr = _ecm.Component(_entity); + auto imu = imuPtr->Data().ImuSensor(); + auto parent = imu->Element()->GetParent(); + if (parent->HasElement("power_load") && parent->HasElement("battery_name")) + { + SensorInfo sensorInfo; + sensorInfo.id = sensorCount; + sensorInfo.name = imuName->Data(); + sensorInfo.powerLoad = parent->Get("power_load"); + sensorInfo.batteryName = parent->Get("battery_name"); + sensorInfo.enableSensor = true; + sensorInfo.dataUpdated = false; + igndbg << "IMU: " << sensorInfo.name << " id: " << sensorInfo.id << std::endl; + igndbg << "IMU: " << sensorInfo.name << " Power: " << sensorInfo.powerLoad << std::endl; + igndbg << "IMU: " << sensorInfo.name << " Battery name: " << sensorInfo.batteryName << std::endl; + igndbg << "IMU: " << sensorInfo.name << " is enabled: " << sensorInfo.enableSensor << std::endl; + igndbg << "IMU: " << sensorInfo.name << " data updated: " << sensorInfo.dataUpdated << std::endl; + this->dataPtr->sensorsInfo.emplace_back(std::move(sensorInfo)); + sensorCount++; + } + } + return true; + }); + + // create a subscription to the sensor topic + for (auto &sensor : this->dataPtr->sensorsInfo) + { + std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/sensor/" + sensor.name + "/activate"}; + auto validSensorTopic = ignition::transport::TopicUtils::AsValidTopic(stateTopic); + if (validSensorTopic.empty()) + { + ignerr << "Failed to create valid topic. Not valid: [" + << sensor.name << "]" << std::endl; + return; + } + std::function callback = std::bind(&SensorPowerSystemPrivate::OnActivateSensor, + this->dataPtr.get(), sensor.id, std::placeholders::_1); + this->dataPtr->node.Subscribe(validSensorTopic, callback); + } +} + +////////////////////////////////////////////////// +void SensorPowerSystemPlugin::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) + +{ + if (_info.paused) + { + return; + } + if (!this->dataPtr->batteriesInitialized) + { + this->dataPtr->batteriesInitialized = true; + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::BatterySoC *_batterySoc, + const ignition::gazebo::components::Name *_name) -> bool + { + if (_name) + { + for (auto &sensor : this->dataPtr->sensorsInfo) + { + if (sensor.batteryName == _name->Data()) + { + igndbg << "Battery found for sensor: " << sensor.name << std::endl; + igndbg << "Battery name: " << _name->Data() << std::endl; + sensor.batteryExist = true; + sensor.batteryEntity = _entity; + } + } + } + return true; + }); + for (auto &sensor : this->dataPtr->sensorsInfo) + { + if (sensor.batteryExist) + { + sensor.batteryConsumerEntity = _ecm.CreateEntity(); + ignition::gazebo::components::BatteryPowerLoadInfo batteryPowerLoad{ + sensor.batteryEntity, sensor.powerLoad}; + _ecm.CreateComponent(sensor.batteryConsumerEntity, ignition::gazebo::components::BatteryPowerLoad(batteryPowerLoad)); + _ecm.SetParentEntity(sensor.batteryConsumerEntity, sensor.batteryEntity); + } + else + { + igndbg << "Sensor " << sensor.name << " battery does not exist" << std::endl; + } + } + } + else + { + for (auto &sensor : this->dataPtr->sensorsInfo) + { + if (sensor.batteryExist && sensor.hasEnoughBattery && sensor.dataUpdated) + { + float setPower = sensor.powerLoad; + if (!sensor.enableSensor) + { + setPower = 0.0; + } + std::lock_guard lock(*sensor.mutex_ptr); + sensor.dataUpdated = false; + ignition::gazebo::v6::components::BatteryPowerLoadInfo batteryPowerLoad{ + sensor.batteryEntity, setPower}; + _ecm.SetComponentData(sensor.batteryConsumerEntity, batteryPowerLoad); + } + } + } +} + +///////////////////////////////////////////////// +void SensorPowerSystemPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) + +{ + if(_info.paused) + { + return; + } + this->dataPtr->HasSufficientBattery(_ecm); +} + +///////////////////////////////////////////////// +void SensorPowerSystemPrivate::HasSufficientBattery( + const ignition::gazebo::EntityComponentManager &_ecm) +{ + _ecm.Each([&]( + const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::BatterySoC *_data + ){ + auto BatteryName = _ecm.Component(_entity); + if(!BatteryName) + { + return false; + } + for(auto &sensor : this->sensorsInfo) + { + if(sensor.batteryName == BatteryName->Data()) + { + if(_data->Data() <= 0.0) + { + sensor.hasEnoughBattery = false; + } + else + { + sensor.hasEnoughBattery = true; + } + } + } + return true; + }); +} + +///////////////////////////////////////////////// +void SensorPowerSystemPrivate::OnActivateSensor(int _id, const ignition::msgs::Boolean &_msg) +{ + std::lock_guard lock(*this->sensorsInfo[_id].mutex_ptr); + this->sensorsInfo[_id].enableSensor = _msg.data(); + this->sensorsInfo[_id].dataUpdated = true; +} + +#include + +IGNITION_ADD_PLUGIN(SensorPowerSystemPlugin, + ignition::gazebo::System, + SensorPowerSystemPlugin::ISystemConfigure, + SensorPowerSystemPlugin::ISystemPreUpdate, + SensorPowerSystemPlugin::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(SensorPowerSystemPlugin, "simulation::SensorPowerSystemPlugin") diff --git a/plugins/SensorPowerSystemPlugin.hh b/plugins/SensorPowerSystemPlugin.hh new file mode 100644 index 0000000..e91c36d --- /dev/null +++ b/plugins/SensorPowerSystemPlugin.hh @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * Copyright (C) 2024 Robin Baran + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SENSOR_POWER_SYSTEM_PLUGIN_HH_ +#define SENSOR_POWER_SYSTEM_PLUGIN_HH_ + + + +#include + + +namespace simulation +{ + // Forward declaration + class SensorPowerSystemPrivate; + + /// \brief A plugin to manage the power of sensors. It reads the power load of each sensor + /// and the battery that powers it. It then adds it as a consumers to the battery. + /// In addition, a topic is exposed to activate or deactivate the sensors. + /// - `/model/{model_name}/sensor/{sensor_name}/activate`: A topic to activate or deactivate the sensor + /// + /// This system does not require any configuration. Although it requires the + /// presence of a Battery plugin in the entity and each sensors should contain the + /// following parameters: + /// - `battery_name`: The name of the battery that powers the sensor + /// - `power_load`: The power load of the sensor + /// TODO(@stevedanomodolor): Ideally this should be part of the sensors system plugin + /// This way not only can we manage the power load of the sensors but also desable the sensors + /// both for the rendering and the physics engine. Lack of public API to do this is the reason + /// why this is a separate plugin. + +class SensorPowerSystemPlugin + : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemPostUpdate + { + // Constructor + public: + SensorPowerSystemPlugin(); + + // Destructor + public: + ~SensorPowerSystemPlugin() override; + + /// Documentation Inherited + public: + void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) final; + /// Documentation Inherited + public: + void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// Documentation Inherited + public: + void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + /// \brief Private data pointer + private: + std::unique_ptr dataPtr; + }; +} + +#endif // SENSOR_POWER_SYSTEM_PLUGIN_HH_ diff --git a/plugins/SolarPanelPlugin.cc b/plugins/SolarPanelPlugin.cc new file mode 100644 index 0000000..e39cc31 --- /dev/null +++ b/plugins/SolarPanelPlugin.cc @@ -0,0 +1,417 @@ +/* + * Copyright (C) 2024 Robin Baran + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "SolarPanelPlugin.hh" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "ignition/gazebo/Model.hh" +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace simulation; + +/// \brief Private data class for SolarPanelPlugin +class simulation::SolarPanelPluginPrivate +{ + /// \brief Get visual children of the link + /// \param[in] _ecm Entity component manager +public: + std::vector GetVisualChildren( + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Find the scene +public: + bool FindScene(); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Event that is used to trigger callbacks when the scene + /// is changed + /// \param[in] _scene The new scene +public: + static ignition::common::EventT + sceneEvent; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + + /// \brief Pointer to rendering scene + /// \param[in] _scene Rendering scene +public: + ignition::rendering::ScenePtr scene{nullptr}; + + /// \brief Connection to the Manager's scene change event. +public: + ignition::common::ConnectionPtr sceneChangeConnection; + + /// \brief Just a mutex for thread safety +public: + std::mutex mutex; + + /// \brief Name of the link +public: + std::string linkName; + + /// \brief Name of the model +public: + std::string modelName; + + /// \brief Name of the topic where the data will be published +public: + std::string topicName; + + /// \brief Power output of the solar panel +public: + double nominalPower{0.0}; + + /// \brief Vector of strings containing the scoped names of the visual children +public: + std::vector scopedVisualChildren; + + /// \brief Model interface +public: + ignition::gazebo::Model model{ignition::gazebo::kNullEntity}; + + /// \brief Link entity +public: + ignition::gazebo::Entity linkEntity{ignition::gazebo::kNullEntity}; + + /// \brief Ignition communication node +public: + ignition::transport::Node node; + + /// \brief Publisher for the radioisotope thermal generator output +public: + ignition::transport::Node::Publisher nominalPowerPub; +}; + +////////////////////////////////////////////////// +SolarPanelPlugin::SolarPanelPlugin() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +SolarPanelPlugin::~SolarPanelPlugin() = default; + +////////////////////////////////////////////////// +void SolarPanelPlugin::Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) +{ + // Store the pointer to the model the solar panel is under + auto model = ignition::gazebo::Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "Solar panel plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->model = model; + this->dataPtr->modelName = model.Name(_ecm); + + // Load params + if (_sdf->HasElement("link_name")) + { + this->dataPtr->linkName = _sdf->Get("link_name"); + this->dataPtr->topicName = "/model/" + this->dataPtr->modelName + "/" + this->dataPtr->linkName + "/solar_panel_output"; + auto validTopic = ignition::transport::TopicUtils::AsValidTopic(this->dataPtr->topicName); + if (validTopic.empty()) + { + ignerr << "Failed to create valid topic [" << this->dataPtr->topicName << "]" << std::endl; + return; + } + // Advertise topic where data will be published + this->dataPtr->nominalPowerPub = this->dataPtr->node.Advertise(validTopic); + } + else + { + ignerr << "Solar panel plugin should have a element. " + << "Failed to initialize." << std::endl; + return; + } + + if (_sdf->HasElement("nominal_power")) + { + this->dataPtr->nominalPower = _sdf->Get("nominal_power"); + } + else + { + ignerr << "Solar panel plugin should have a element. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->sceneChangeConnection = this->dataPtr->sceneEvent.Connect(std::bind(&SolarPanelPlugin::SetScene, this, std::placeholders::_1)); +} + +////////////////////////////////////////////////// +void SolarPanelPlugin::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + if (_info.paused) + { + return; + } + if (!this->dataPtr->scene) + { + if (!this->dataPtr->FindScene()) + { + ignwarn << "Rendering scene not available yet" << std::endl; + return; + } + } + + std::shared_ptr rayQuery = this->dataPtr->scene->CreateRayQuery(); + if (!rayQuery) + { + ignerr << "Failed to create RayQuery" << std::endl; + return; + } + + if (this->dataPtr->scopedVisualChildren.empty()) + { + this->dataPtr->scopedVisualChildren = this->dataPtr->GetVisualChildren(_ecm); + } + + // Get sun entity + ignition::gazebo::Entity sunEntity; + ignition::math::Pose3d sunPose; + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Name *_name, + const ignition::gazebo::components::Pose *_pose) -> bool + { + if (_name->Data() == "sun") + { + sunEntity = _entity; + sunPose = _pose->Data(); + return false; // Stop iteration + } + return true; + }); + + if (sunEntity == ignition::gazebo::v6::kNullEntity) + { + ignerr << "Sun entity not found" << std::endl; + return; + } + + // Check if sun entity is of type "light" and has a "direction" element + const auto *lightComp = _ecm.Component(sunEntity); + ignition::math::Vector3d direction; + if (lightComp) + { + const auto &light = lightComp->Data(); + direction = light.Direction(); + } + else + { + ignerr << "Sun entity is not a light!" << std::endl; + return; + } + + // Rotate sun direction according to sun pose orientation + ignition::math::Vector3d sunDirection = sunPose.Rot().RotateVector(direction); + + if (this->dataPtr->linkEntity == ignition::gazebo::v6::kNullEntity) + { + this->dataPtr->linkEntity = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + } + + ignition::math::Pose3d linkPose = ignition::gazebo::worldPose(this->dataPtr->linkEntity, _ecm); + // Perform ray cast from link to sun + ignition::math::Vector3d start = linkPose.Pos(); + ignition::math::Vector3d end = sunPose.Pos(); + + rayQuery->SetOrigin(end); + rayQuery->SetDirection(start - end); + + // Check if ray intersects with any obstacles + auto result = rayQuery->ClosestPoint(); + bool isValid = result; + + std::string objectName = "unknown"; + bool isInLOS = false; + ignition::rendering::NodePtr node = this->dataPtr->scene->NodeById(result.objectId); + if (node) + { + objectName = node->Name(); + if (isValid) + { + isInLOS = (any_of(this->dataPtr->scopedVisualChildren.begin(), this->dataPtr->scopedVisualChildren.end(), [&](const std::string &elem) + { return elem == objectName; })); + } + } + + // Compute current power output + float currentPower = 0.0F; + + // Compute the angle between the link normal and sun direction + // Calculate dot product + ignition::math::Vector3d linkNormal = linkPose.Rot().RotateVector(ignition::math::Vector3d::UnitZ); + float dotProduct = linkNormal.Dot(-sunDirection); // Negate sunDirection because it points from sun to scene + + // Solar panel will not receive any power if angle is more than 90deg (sun rays hitting horizontally or below) + if ((dotProduct > 0.0F) && (isInLOS)) + { + // Calculate magnitudes + float magnitude1 = linkNormal.Length(); + float magnitude2 = sunDirection.Length(); + // Calculate cosine of the angle + float cosAngle; + if (ignition::math::equal(magnitude1, 0.0F) || + ignition::math::equal(magnitude2, 0.0F)) + { + cosAngle = 1.0F; + } + else + { + cosAngle = dotProduct / (magnitude1 * magnitude2); + } + + // Compute the effective area factor (cosine of the angle) + float effectiveAreaFactor = std::max(0.0F, cosAngle); + + // Compute current power based on the angle + currentPower = this->dataPtr->nominalPower * effectiveAreaFactor; + } + + // Publish result + ignition::msgs::Float msg; + msg.set_data(currentPower); + this->dataPtr->nominalPowerPub.Publish(msg); + + igndbg << "Solar Panel Plugin:: Current power output: " << currentPower << " watts" << std::endl; + igndbg << "Solar Panel Plugin:: In line of sight: " << (isInLOS ? "Yes" : "No") << std::endl; +} + +////////////////////////////////////////////////// +void SolarPanelPlugin::SetScene(ignition::rendering::ScenePtr _scene) +{ + std::lock_guard lock(this->dataPtr->mutex); + // APIs make it possible for the scene pointer to change + if (this->dataPtr->scene != _scene) + { + this->dataPtr->scene = _scene; + } +} + +////////////////////////////////////////////////// +bool SolarPanelPluginPrivate::FindScene() +{ + auto loadedEngNames = ignition::rendering::loadedEngines(); + if (loadedEngNames.empty()) + { + ignwarn << "No rendering engine is loaded yet" << std::endl; + return false; + } + + // assume there is only one engine loaded + auto engineName = loadedEngNames[0]; + if (loadedEngNames.size() > 1) + { + ignwarn << "More than one engine is available. " + << "Using engine [" << engineName << "]" << std::endl; + } + auto engine = ignition::rendering::engine(engineName); + if (!engine) + { + ignerr << "Internal error: failed to load engine [" << engineName + << "]. Solar panel plugin won't work." << std::endl; + return false; + } + + if (engine->SceneCount() == 0) + { + igndbg << "No scene has been created yet" << std::endl; + return false; + } + + // Get first scene + auto scenePtr = engine->SceneByIndex(0); + if (nullptr == scenePtr) + { + ignerr << "Internal error: scene is null." << std::endl; + return false; + } + + if (engine->SceneCount() > 1) + { + igndbg << "More than one scene is available. " + << "Using scene [" << scene->Name() << "]" << std::endl; + } + + if (!scenePtr->IsInitialized() || nullptr == scenePtr->RootVisual()) + { + return false; + } + + this->scene = scenePtr; + return true; +} + +////////////////////////////////////////////////// +std::vector SolarPanelPluginPrivate::GetVisualChildren( + const ignition::gazebo::EntityComponentManager &_ecm) +{ + // Build the prefix for the scoped name + std::string scopedPrefix = this->modelName + "::" + this->linkName + "::"; + + // Find all visual entities that are children of this link + std::vector scopedVisualChildren; + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Visual *, + const ignition::gazebo::components::Name *_name, + const ignition::gazebo::components::ParentEntity *_parent) -> bool + { + if (_parent->Data() == linkEntity) + { + std::string scopedName = scopedPrefix + _name->Data(); + scopedVisualChildren.push_back(scopedName); + } + return true; + }); + + return scopedVisualChildren; +} + +ignition::common::EventT + SolarPanelPluginPrivate::sceneEvent; + +IGNITION_ADD_PLUGIN(SolarPanelPlugin, ignition::gazebo::System, + SolarPanelPlugin::ISystemConfigure, + SolarPanelPlugin::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(SolarPanelPlugin, "simulation::SolarPanelPlugin") diff --git a/plugins/SolarPanelPlugin.hh b/plugins/SolarPanelPlugin.hh new file mode 100644 index 0000000..8c3028a --- /dev/null +++ b/plugins/SolarPanelPlugin.hh @@ -0,0 +1,69 @@ +/* + * Copyright (C) 2024 Robin Baran + * Copyright (C) 2024 Stevedan Ogochukwu Omodolor Omodia + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SOLAR_PANEL_PLUGIN_HH_ +#define SOLAR_PANEL_PLUGIN_HH_ + +#include +#include +#include + +#include +#include + +namespace simulation +{ + // Forward declaration + class SolarPanelPluginPrivate; + + /// \brief SolarPanelPlugin SolarPanelPlugin.hh + /// \brief A plugin that simulates a solar panel + class SolarPanelPlugin : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPostUpdate + { + + /// \brief Constructor + public: + SolarPanelPlugin(); + + /// \brief Destructor + public: + ~SolarPanelPlugin() override; + + /// Documentation inherited + public: + void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + /// Documentation inherited + public: + void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) final; + + /// \brief Set the scene + public: + void SetScene(ignition::rendering::ScenePtr _scene); + + /// \brief Private data pointer + private: + std::unique_ptr dataPtr; + }; +} +#endif // SOLAR_PANEL_PLUGIN_HH_