diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..8186d4ea --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,23 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/nxfrst/x-storage/atom_robotics/space_ros_submissions/space_ros_ws/install/canadarm/include/**", + "/home/nxfrst/x-storage/combat_robotics/arista_test_ws/install/arista_interfaces/include/**", + "/home/nxfrst/x-storage/combat_robotics/arista_test_ws/install/arista_control_stream/include/**", + "/opt/ros/humble/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..84b8123b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,16 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/nxfrst/x-storage/atom_robotics/space_ros_submissions/space_ros_ws/install/mars_rover/local/lib/python3.10/dist-packages", + "/home/nxfrst/x-storage/atom_robotics/space_ros_submissions/space_ros_ws/install/canadarm/local/lib/python3.10/dist-packages", + "/home/nxfrst/x-storage/combat_robotics/arista_test_ws/install/arista_interfaces/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/nxfrst/x-storage/atom_robotics/space_ros_submissions/space_ros_ws/install/mars_rover/local/lib/python3.10/dist-packages", + "/home/nxfrst/x-storage/atom_robotics/space_ros_submissions/space_ros_ws/install/canadarm/local/lib/python3.10/dist-packages", + "/home/nxfrst/x-storage/combat_robotics/arista_test_ws/install/arista_interfaces/local/lib/python3.10/dist-packages", + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/custom_gz_plugins/CMakeLists.txt b/custom_gz_plugins/CMakeLists.txt new file mode 100644 index 00000000..f34126c1 --- /dev/null +++ b/custom_gz_plugins/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(custom_gz_plugins) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_subdirectory(src/DayLightManager) +add_subdirectory(src/DustManager) +add_subdirectory(src/VehicleDust) +add_subdirectory(src/DroneDustPlugin) + +add_executable(inguenity_control nodes/inguenity_control.cpp) +ament_target_dependencies(inguenity_control rclcpp std_msgs) + +install(TARGETS +inguenity_control +DESTINATION lib/${PROJECT_NAME}) + +## Install the launch directory +install( + DIRECTORY launch worlds config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env_hooks/${PROJECT_NAME}.dsv.in") + +ament_package() diff --git a/custom_gz_plugins/Dockerfile b/custom_gz_plugins/Dockerfile new file mode 100644 index 00000000..ffbd0d87 --- /dev/null +++ b/custom_gz_plugins/Dockerfile @@ -0,0 +1,108 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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. +# +# A Docker configuration script to build the Space ROS image. +# +# The script provides the following build arguments: +# +# VCS_REF - The git revision of the Space ROS source code (no default value). +# VERSION - The version of Space ROS (default: "preview") + +FROM osrf/space-ros:latest + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="Real World Extraterrestrial Simulations" +LABEL org.label-schema.description="Realistic Extraterrestrial Simulations demos using custom GZ plugins & data from NASA PDS" +LABEL org.label-schema.vendor="Open Robotics" +LABEL org.label-schema.version=${VERSION} +LABEL org.label-schema.url="https://github.com/space-ros" +LABEL org.label-schema.vcs-url="https://github.com/space-ros/docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a few key variables +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV GZ_VERSION=harmonic + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive + +# Clone all space-ros sources +RUN mkdir ${SPACEROS_DIR}/src \ + && vcs import ${SPACEROS_DIR}/src < ${SPACEROS_DIR}/exact.repos + +# Make sure the latest versions of packages are installed +# Using Docker BuildKit cache mounts for /var/cache/apt and /var/lib/apt ensures that +# the cache won't make it into the built image but will be maintained between steps. +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get dist-upgrade -y +RUN rosdep update + +# Get rosinstall_generator +# Using Docker BuildKit cache mounts for /var/cache/apt and /var/lib/apt ensures that +# the cache won't make it into the built image but will be maintained between steps. +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y && sudo apt-get install -y python3-rosinstall-generator + +RUN mkdir -p ${DEMO_DIR}/src +WORKDIR ${DEMO_DIR} + +# Install libmongoc for development +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install libmongoc-dev -y + +# Compile mongo cxx driver https://mongocxx.org/mongocxx-v3/installation/linux/ +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install libssl-dev build-essential devscripts debian-keyring fakeroot debhelper cmake libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen -y +RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz +RUN tar -xzf mongo-cxx-driver-r3.6.7.tar.gz +RUN cd mongo-cxx-driver-r3.6.7/build && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local && sudo cmake --build . --target EP_mnmlstc_core && cmake --build . && sudo cmake --build . --target install + +# Add osrf packages repository and manually install gz sim harmonic dependecies as they haven't been added to the rosdep index as of yet +RUN sudo curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg +RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null +RUN sudo apt-get update && sudo apt-get install -y libgz-math7-dev libgz-transport13-dev libgz-sim8-dev libgz-msgs10-dev libgz-plugin2-dev + +# Get the source for the dependencies +COPY --chown=${USERNAME}:${USERNAME} dependencies_source_pkgs.repos /tmp/ +RUN vcs import src < /tmp/dependencies_source_pkgs.repos && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ +--mount=type=cache,target=/var/lib/apt,sharing=locked \ +sudo apt-get update -y \ +&& /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ +&& rosdep install --from-paths ${SPACEROS_DIR}/src src --ignore-src -r -y --rosdistro ${ROSDISTRO} --skip-keys "console_bridge generate_parameter_library fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers rmw_connextdds ros_testing rmw_connextdds rmw_fastrtps_cpp rmw_fastrtps_dynamic_cpp composition demo_nodes_py lifecycle rosidl_typesupport_fastrtps_cpp rosidl_typesupport_fastrtps_c ikos diagnostic_aggregator diagnostic_updater joy qt_gui rqt_gui rqt_gui_py" + +# Build the demo +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release' + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/custom_gz_plugins/README.md b/custom_gz_plugins/README.md new file mode 100644 index 00000000..4ab331b1 --- /dev/null +++ b/custom_gz_plugins/README.md @@ -0,0 +1,109 @@ +# NASA Space ROS Sim Summer Sprint Challenge + +Team Lead Freelancer Username: @utkrishtjain + +Submission Title: Custom Models & Gazebo Plugins for Realistic Simulations + +## Description and Features + +### Day Light Manager Plugin +The Sun is a crucial factor in driving most planetary processes, affecting nearly every environmental aspect either directly or indirectly. For realistic environmental simulations, accurately modeling the Sun's movement is vital, particularly in complex scenarios. The Daylight Manager plugin in Gazebo simulates the Sun's path based on the time of day and the observer's latitude, offering a dynamic and lifelike portrayal of sunlight. As the Sun's position changes, the shadows cast by objects and the background color also shift, as illustrated below. +> For detailed description of the plugin and it's usage, click [here](src/DayLightManager/README.md). + +![Day Light Manager](assets/time_of_day.gif) + + +### Dust Manager Plugin +The Dust Manager plugin enables the simulation of realistic dust behavior in Martian or space environments by managing particle emitters. Dust plays a critical role in extraterrestrial settings, especially on Mars, where it can impact visibility, equipment functionality, and the overall success of missions. This plugin allows for precise simulation of dust clouds, supporting the development and testing of vehicles and systems intended for planetary exploration. By replicating how dust interacts with moving objects, it helps create more authentic and challenging scenarios for space missions. Additionally, the Dust Manager Plugin offers the ability to control wind conditions based on the parameters of dust storms incorporated into the simulation. +> For detailed description of the plugin and it's usage, click [here](src/DustManager/README.md). + +![Dust Manager Gui](assets/dust_gui.gif) + +### Car Dust Plugin +The Dust Emission Plugin simulates dust clouds generated by heavy vehicles moving across dusty terrains, such as the surfaces of Mars or the Moon. As vehicles traverse these environments, the plugin dynamically controls particle emitters to mimic realistic dust behavior, taking into account vehicle speed, surface texture, and atmospheric conditions. This enhances the accuracy of simulations for planetary exploration by providing lifelike interactions between vehicles and dust, aiding in the design and testing of exploration systems. + +> For detailed description of the plugin and it's usage, click [here](src/VehicleDust/README.md) + +![Car Dust](assets/car_dust.gif) + +### Drone Dust Plugin +The Drone Dust plugin simulates the dust clouds dispersed by a drone's thrusters during takeoff and landing in dusty environments, such as on Mars or the Moon. It dynamically controls particle emitters based on the drone's movement and thruster activity, creating realistic dust dispersion effects. This plugin helps in evaluating how dust affects the drone's performance and surrounding environment, enabling more accurate simulations for planetary exploration and vehicle testing. + +> For detailed description of the plugin and it's usage, click [here](src/DroneDustPlugin/README.md) + +![Drone Dust](assets/drone_dust.gif) + + + +### Real Surface Models +Custom models made from real elevation data (From NASA's Planetary Data System) of celestial bodies like Moon and Mars is also added. + +- Model of Lunar Terrain made from elevation data LOLA on board the Lunar Reconnaissance Orbiter (LRO) + +- Models of Martian Terrain made from elevation data from HiRISE Images on board the Mars Reconnaissance Orbiter (MRO) + +Mars +Moon + +## Build and Run + +- Build the Custom Docker image with all the custom dependencies using the following command + +```shell +./build.sh +``` + +- Once the Image has been build, use the following command to run the container + +```shell +./run.sh +``` +This will run the container where you can launch the simulation. +> If the container is running, you can use the same script/command in another terminal instance to attach a terminal into the docker. + +### GUI PLUGINS +- Once the docker container is up, you can use the demo launch file to launch the simualtion using the following command. + +```shell +ros2 launch custom_gz_plugins demo_world.launch.py +``` +> In order to user the GUI Plugin: DustManger or DayLightManger, use to menu option in the simulation (Top Right Corner) to load the plugin + +### System Plugins + +#### Vehicle Dust +- Inside the docker container, navigate to the worlds directory using the following command +```bash +cd /home/spaceros-user/demos_ws/src/demos/custom_gz_plugins/worlds +``` +- Use the follwing command to launch the world in GZ SIM + +```bash +gz sim -r vehicle_dust_demo.sdf +``` +- Use the teleop GUI plugin in GZ SIM to drive the robot im order to see the dust effect in the simulation + +#### Drone Dust +- Inside the docker container, navigate to the worlds directory using the following command +```bash +cd /home/spaceros-user/demos_ws/src/demos/custom_gz_plugins/worlds +``` +- Use the follwing command to launch the world in GZ SIM + +```bash +gz sim -r vehicle_dust_demo.sdf +``` + +- Run the follwoing command to launch the ros-gz bridges +```bash +ros2 launch custom_gz_plugins ingenuity.launch.py +``` + +- Run the following command to start the teleop node of ingenuity. +```bash +ros2 run custom_gz_plugins inguenity_control +``` +- Use the teleop node to fly the ingenuity robot im order to see the dust effect in the simulation + + +> Note: These simulations are just to demonstate the plugins features so the dynamics and physics of the models might not be accurate. \ No newline at end of file diff --git a/custom_gz_plugins/assets/car_dust.gif b/custom_gz_plugins/assets/car_dust.gif new file mode 100644 index 00000000..76b31fe1 Binary files /dev/null and b/custom_gz_plugins/assets/car_dust.gif differ diff --git a/custom_gz_plugins/assets/drone_dust.gif b/custom_gz_plugins/assets/drone_dust.gif new file mode 100644 index 00000000..484ad01d Binary files /dev/null and b/custom_gz_plugins/assets/drone_dust.gif differ diff --git a/custom_gz_plugins/assets/dust_gui.gif b/custom_gz_plugins/assets/dust_gui.gif new file mode 100644 index 00000000..4c763a93 Binary files /dev/null and b/custom_gz_plugins/assets/dust_gui.gif differ diff --git a/custom_gz_plugins/assets/mars_world.png b/custom_gz_plugins/assets/mars_world.png new file mode 100644 index 00000000..d0719dbf Binary files /dev/null and b/custom_gz_plugins/assets/mars_world.png differ diff --git a/custom_gz_plugins/assets/moon_world.png b/custom_gz_plugins/assets/moon_world.png new file mode 100644 index 00000000..685bd868 Binary files /dev/null and b/custom_gz_plugins/assets/moon_world.png differ diff --git a/custom_gz_plugins/assets/time_of_day.gif b/custom_gz_plugins/assets/time_of_day.gif new file mode 100644 index 00000000..df3ada13 Binary files /dev/null and b/custom_gz_plugins/assets/time_of_day.gif differ diff --git a/custom_gz_plugins/build.sh b/custom_gz_plugins/build.sh new file mode 100755 index 00000000..dc014cdb --- /dev/null +++ b/custom_gz_plugins/build.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash + +ORG=spaceros +IMAGE=custom_sim_custom_plugins_demo +TAG=latest + +VCS_REF="" +VERSION=preview + +# Exit script with failure if build fails +set -eo pipefail + +echo "" +echo "##### Building Docker Image #####" +echo "" + +docker build -t $ORG/$IMAGE:$TAG \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" diff --git a/custom_gz_plugins/config/buggy_bridge.yaml b/custom_gz_plugins/config/buggy_bridge.yaml new file mode 100644 index 00000000..e69de29b diff --git a/custom_gz_plugins/config/ingenuity_bridge.yaml b/custom_gz_plugins/config/ingenuity_bridge.yaml new file mode 100644 index 00000000..184a28e1 --- /dev/null +++ b/custom_gz_plugins/config/ingenuity_bridge.yaml @@ -0,0 +1,24 @@ + +- ros_topic_name: "/thrust" + gz_topic_name: "/model/ingenuity/joint/bottomblade_joint/cmd_thrust" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "gz.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/ang_vel" + gz_topic_name: "/model/ingenuity/joint/bottomblade_joint/ang_vel" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "gz.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/upper_blade" + gz_topic_name: "/plate1_joint/cmd_pos" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "gz.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/lower_blade" + gz_topic_name: "/plate2_joint/cmd_pos" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "gz.msgs.Double" + direction: ROS_TO_GZ diff --git a/custom_gz_plugins/dependencies_source_pkgs.repos b/custom_gz_plugins/dependencies_source_pkgs.repos new file mode 100644 index 00000000..f94fb1fb --- /dev/null +++ b/custom_gz_plugins/dependencies_source_pkgs.repos @@ -0,0 +1,130 @@ +repositories: + demos: + type: git + url: https://github.com/jasmeet0915/demos + version: jasmeet/custom_gz_plugins_pkg + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control.git + version: humble + ackermann_msgs: + type: git + url: https://github.com/ros-drivers/ackermann_msgs.git + version: ros2 + resource_retriever: + type: git + url: https://github.com/ros/resource_retriever.git + version: humble + angles: + type: git + url: https://github.com/ros/angles.git + version: humble-devel + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + control_msgs: + type: git + url: https://github.com/ros-controls/control_msgs.git + version: humble + gz_ros2_control: + type: git + url: https://github.com/jasmeet0915/gz_ros2_control.git + version: jasmeet/humble_harmonic_support + backward_ros: + type: git + url: https://github.com/pal-robotics/backward_ros.git + version: foxy-devel + qt_gui_core: + type: git + url: https://github.com/ros-visualization/qt_gui_core.git + version: humble + python_qt_binding: + type: git + url: https://github.com/ros-visualization/python_qt_binding.git + version: humble + ros2_controllers: + type: git + url: https://github.com/tonylitianyu/ros2_controllers.git + version: effort_group_position_controller_2 + actuator_msgs: + type: git + url: https://github.com/rudislabs/actuator_msgs.git + version: main + filters: + type: git + url: https://github.com/ros/filters.git + version: ros2 + rviz: + type: git + url: https://github.com/ros2/rviz.git + version: humble + ros_gz: + type: git + url: https://github.com/gazebosim/ros_gz.git + version: humble + generate_parameter_library: + type: git + url: https://github.com/PickNikRobotics/generate_parameter_library.git + version: main + joint_state_publisher: + type: git + url: https://github.com/ros/joint_state_publisher.git + version: ros2 + rsl: + type: git + url: https://github.com/PickNikRobotics/RSL.git + version: main + cpp_polyfills: + type: git + url: https://github.com/PickNikRobotics/cpp_polyfills.git + version: main + image_transport: + type: git + url: https://github.com/ros-perception/image_common.git + version: humble + interactive_markers: + type: git + url: https://github.com/ros-visualization/interactive_markers.git + version: humble + laser_geometry: + type: git + url: https://github.com/ros-perception/laser_geometry.git + version: humble + simulation: + type: git + url: https://github.com/jasmeet0915/simulation + version: plugin_models + warehouse_ros: + type: git + url: https://github.com/moveit/warehouse_ros.git + version: ros2 + ros-humble-warehouse-ros-mongo: + type: git + url: https://github.com/ros-planning/warehouse_ros_mongo.git + version: ros2 + vision_msgs: + type: git + url: https://github.com/ros-perception/vision_msgs.git + version: ros2 + navigation_msgs: + type: git + url: https://github.com/ros-planning/navigation_msgs.git + version: humble + gps_msgs: + type: git + url: https://github.com/swri-robotics/gps_umd.git + path: gps_msgs + version: 113782d + yaml_cpp_vendor: + type: git + url: https://github.com/ros2/yaml_cpp_vendor.git + version: humble + xacro: + type: git + url: https://github.com/ros/xacro.git + version: ros2 diff --git a/custom_gz_plugins/entrypoint.sh b/custom_gz_plugins/entrypoint.sh new file mode 100755 index 00000000..7f15d890 --- /dev/null +++ b/custom_gz_plugins/entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# Setup the Demo environment +source "${SPACEROS_DIR}/install/setup.bash" +exec "$@" diff --git a/custom_gz_plugins/env_hooks/custom_gz_plugins.dsv.in b/custom_gz_plugins/env_hooks/custom_gz_plugins.dsv.in new file mode 100644 index 00000000..65bffe3e --- /dev/null +++ b/custom_gz_plugins/env_hooks/custom_gz_plugins.dsv.in @@ -0,0 +1,2 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib +prepend-non-duplicate;GZ_GUI_PLUGIN_PATH;lib \ No newline at end of file diff --git a/custom_gz_plugins/launch/buggy_dust_demo.launch.py b/custom_gz_plugins/launch/buggy_dust_demo.launch.py new file mode 100644 index 00000000..e45d5446 --- /dev/null +++ b/custom_gz_plugins/launch/buggy_dust_demo.launch.py @@ -0,0 +1,34 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node + +def generate_launch_description(): + # Get directories + pkg_ugv_sim = get_package_share_directory('custom_gz_plugins') + + # Set robot name and bridge config + robot_name = 'buggy' + bridge_config = os.path.join(pkg_ugv_sim, 'config', robot_name + '_bridge.yaml') + + # Launch Gazebo + gazebo = ExecuteProcess( + cmd=['gz', 'sim', '-r', 'vehicle_dust_demo.sdf'], + output='screen' + ) + + # Launch the ROS-GZ bridge + ros_gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + parameters=[{'config_file': bridge_config}], + output='screen' + ) + + return LaunchDescription([ + gazebo, + ros_gz_bridge + ]) \ No newline at end of file diff --git a/custom_gz_plugins/launch/demo_world.launch.py b/custom_gz_plugins/launch/demo_world.launch.py new file mode 100644 index 00000000..316b5974 --- /dev/null +++ b/custom_gz_plugins/launch/demo_world.launch.py @@ -0,0 +1,169 @@ +from http.server import executable +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription +from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit, OnExecutionComplete +import os +from os import environ + +from ament_index_python.packages import get_package_share_directory + +import xacro + + +def generate_launch_description(): + mars_rover_demos_path = get_package_share_directory('mars_rover') + mars_rover_models_path = get_package_share_directory('simulation') + + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), + environ.get('LD_LIBRARY_PATH', default='')]), + 'GZ_SIM_RESOURCE_PATH': + ':'.join([mars_rover_demos_path, + mars_rover_models_path + '/models', + mars_rover_demos_path + '/worlds'])} + + urdf_model_path = os.path.join(mars_rover_models_path, 'models', 'curiosity_path', + 'urdf', 'curiosity_mars_rover.xacro.urdf') + + doc = xacro.process_file(urdf_model_path) + robot_description = {'robot_description': doc.toxml()} + + declare_world_name_arg = DeclareLaunchArgument('world_name', default_value='mars_gale_crater_patch1.sdf', + description='Name of the world file to launch in Gazebo') + world_name = LaunchConfiguration('world_name') + + arm_node = Node( + package="mars_rover", + executable="move_arm", + output='screen' + ) + + mast_node = Node( + package="mars_rover", + executable="move_mast", + output='screen' + ) + + wheel_node = Node( + package="mars_rover", + executable="move_wheel", + output='screen' + ) + + run_node = Node( + package="mars_rover", + executable="run_demo", + output='screen' + ) + + start_world = ExecuteProcess( + cmd=['gz sim', world_name, '-r'], + output='screen', + additional_env=env, + shell=True + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[robot_description]) + + spawn = Node( + package='ros_gz_sim', executable='create', + arguments=[ + '-name', 'curiosity_path', + '-topic', robot_description, + "-z", "4", + "-x", "0", + "-y", "0", + "-Y", "0", + ], + output='screen' + ) + + ## Control Components + + component_state_msg = '{name: "GazeboSimSystem", target_state: {id: 3, label: ""}}' + + ## a hack to resolve current bug + set_hardware_interface_active = ExecuteProcess( + cmd=['ros2', 'service', 'call', + 'controller_manager/set_hardware_component_state', + 'controller_manager_msgs/srv/SetHardwareComponentState', + component_state_msg] + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_arm_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'arm_joint_trajectory_controller'], + output='screen' + ) + + load_mast_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'mast_joint_trajectory_controller'], + output='screen' + ) + + load_wheel_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'wheel_velocity_controller'], + output='screen' + ) + + load_steer_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'steer_position_controller'], + output='screen' + ) + + load_suspension_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'wheel_tree_position_controller'], + output='screen' + ) + + return LaunchDescription([ + declare_world_name_arg, + start_world, + robot_state_publisher, + spawn, + arm_node, + mast_node, + wheel_node, + run_node, + RegisterEventHandler( + OnProcessExit( + target_action=spawn, + on_exit=[set_hardware_interface_active], + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=set_hardware_interface_active, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_arm_joint_traj_controller, + load_mast_joint_traj_controller, + load_wheel_joint_traj_controller, + load_steer_joint_traj_controller, + load_suspension_joint_traj_controller], + ) + ), + ]) \ No newline at end of file diff --git a/custom_gz_plugins/launch/ingenuity.launch.py b/custom_gz_plugins/launch/ingenuity.launch.py new file mode 100644 index 00000000..566c7136 --- /dev/null +++ b/custom_gz_plugins/launch/ingenuity.launch.py @@ -0,0 +1,42 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.actions import Node + +def generate_launch_description(): + # Get directories + custom_pkg = get_package_share_directory('custom_gz_plugins') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + # Set robot name and bridge config + robot_name = 'ingenuity' + world_name = 'drone_dust_demo' + bridge_config = os.path.join(custom_pkg, 'config', robot_name + '_bridge.yaml') + + world_path = os.path.join(custom_pkg, 'worlds', world_name + '.sdf') + + # launch GZ Sim with empty world + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={ + 'gz_args' : world_path + " -r" + }.items() + ) + + # Launch the ROS-GZ bridge + ros_gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + parameters=[{'config_file': bridge_config}], + output='screen' + ) + + return LaunchDescription([ + # gz_sim, + ros_gz_bridge + ]) \ No newline at end of file diff --git a/custom_gz_plugins/nodes/inguenity_control.cpp b/custom_gz_plugins/nodes/inguenity_control.cpp new file mode 100644 index 00000000..7db2da41 --- /dev/null +++ b/custom_gz_plugins/nodes/inguenity_control.cpp @@ -0,0 +1,184 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include +#include +#include + +#include +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + + +int getch(void) +{ + int ch; + struct termios oldt; + struct termios newt; + + // Store old settings, and copy to new settings + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + + // Make required changes and apply the settings + newt.c_lflag &= ~(ICANON | ECHO); + newt.c_iflag |= IGNBRK; + newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF); + newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN); + newt.c_cc[VMIN] = 1; + newt.c_cc[VTIME] = 0; + tcsetattr(fileno(stdin), TCSANOW, &newt); + + // Get the current character + ch = getchar(); + + // Reapply old settings + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + + return ch; +} + +int main(int argc, char * argv[]) +{ +const char* msg = R"( +Reading from the keyboard! +--------------------------- +Thrust Key: + w + + s + +Lower Blades: + +a d + + +Upper Blades: + u + + j + +t : up (+z) +b : down (-z) +q/e : reverse thrust +w/s : increase/decrease Thrust Key by 10 +a/d : increase/decrease Lower Blade angle by 1 +u/j : increase/decrease Upper Blade angle by 1 +NOTE : Increasing or Decreasing will take affect live on the moving robot. + Consider Stopping the robot before changing it. +CTRL-C to quit +)"; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("uuv_teleop"); + std::cout<create_publisher("/lower_blade", 10); + auto publisher_hoz = node->create_publisher("/upper_blade", 10); + auto publisher_thrust = node->create_publisher("/thrust", 10); + auto message_ver = std_msgs::msg::Float64(); + auto message_hoz = std_msgs::msg::Float64(); + auto message_thrust = std_msgs::msg::Float64(); + message_ver.data = 0; + message_hoz.data = 0; + message_thrust.data = 0; + + while (rclcpp::ok()) + { + char key = getch(); + + if(key == 'A'|| key == 'a'){ + if (message_ver.data <= -1.0){ + message_ver.data = message_ver.data; + std::cout<<"Lower Blade Angle:- "<publish(message_ver); + std::cout<<"Lower Blade Angle:- "<= 1.0){ + message_ver.data = message_ver.data; + std::cout<<"Lower Blade Angle:- "<publish(message_ver); + std::cout<<"Lower Blade Angle:- "<publish(message_hoz); + std::cout<<"Upper Blade Angle:- "<= 1.0){ + message_hoz.data = message_hoz.data; + std::cout<<"Upper Blade Angle:- "<publish(message_hoz); + std::cout<<"Upper Blade Angle:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<=0){ + message_thrust.data = message_thrust.data; + publisher_thrust->publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value:- "<publish(message_thrust); + std::cout<<"Thrust Value (UP):- "<publish(message_thrust); + std::cout<<"Thrust Value (DOWN):- "< + + + custom_gz_plugins + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/custom_gz_plugins/run.sh b/custom_gz_plugins/run.sh new file mode 100755 index 00000000..e99e07d8 --- /dev/null +++ b/custom_gz_plugins/run.sh @@ -0,0 +1,36 @@ +#!/bin/bash + +DOCKER_ARGS+=("-e NVIDIA_VISIBLE_DEVICES=all") +DOCKER_ARGS+=("-e NVIDIA_DRIVER_CAPABILITIES=all") + +xhost +local:root + +ORG=spaceros +IMAGE=custom_sim_custom_plugins_demo +TAG=latest + +container_name="space_ros_custom_gz_plugins_demo" + +ros_domain_id=$(printenv ROS_DOMAIN_ID) + +# Check if a Docker container with "$container_name" is already running. +# If it is running, open an interactive bash shell inside it. +# If the container is not running, create and run a new Docker container with specified configurations +if docker ps --format '{{.Names}}' | grep -q "$container_name"; then + docker exec -it "$container_name" /bin/bash +else + docker run -it --rm \ + ${DOCKER_ARGS[@]} \ + -e DISPLAY=$DISPLAY \ + -v $PWD/..:/workspaces/sim_ws/src \ + -v /var/run/docker.sock:/var/run/docker.sock \ + --name "$container_name" \ + --workdir /home/spaceros-user/demos_ws/src \ + --env ROS_DOMAIN_ID=$ros_domain_id \ + --runtime nvidia \ + --network host \ + -v /dev/input:/dev/input --device-cgroup-rule='c 13:* rmw' \ + $@ \ + $ORG/$IMAGE:$TAG \ + /bin/bash +fi diff --git a/custom_gz_plugins/src/DayLightManager/CMakeLists.txt b/custom_gz_plugins/src/DayLightManager/CMakeLists.txt new file mode 100644 index 00000000..2589ba2d --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +if(POLICY CMP0100) + cmake_policy(SET CMP0100 NEW) +endif() + +project(DayLightManager) + +set(CMAKE_AUTOMOC ON) + +find_package(gz-sim8 REQUIRED COMPONENTS gui) + +QT5_ADD_RESOURCES(resources_RCC ${PROJECT_NAME}.qrc) + +add_library(${PROJECT_NAME} SHARED + ${PROJECT_NAME}.cc + ${resources_RCC} +) +target_link_libraries(${PROJECT_NAME} + PRIVATE gz-sim8::gui +) + +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) \ No newline at end of file diff --git a/custom_gz_plugins/src/DayLightManager/DayLightManager.cc b/custom_gz_plugins/src/DayLightManager/DayLightManager.cc new file mode 100644 index 00000000..faa43753 --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/DayLightManager.cc @@ -0,0 +1,627 @@ +/* + * Jayesh Chaudhary + * + * 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 "DayLightManager.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +namespace gz::sim::plugins +{ + // Private data structure for DayLightManager + class DayLightManagerPrivate + { + public: + // Gz setup variables + gz::transport::Node _node; + gz::msgs::Time _timeMsg; + gz::transport::Node::Publisher _pub; + + std::string _worldName; + std::string _sunModelName{"sun_sphere"}; + std::string _serviceName; + + int _gzMsgsTimeout; + + std::string _planetSelect; + gz::msgs::Pose _sunSpherePose; + gz::msgs::Boolean _sunSpherePoseRes; + bool _sunSpherePoseResult; + + // Code essential variables + bool _init; + + int _curntRate; + int _updateRateCnt; + const int _updateRate{100}; + + // Pointers and variables to hold current configurations + int _totalMin; + + bool _updateSky; + + float **_selectPlanet; + + /* Planetary Parameters */ + + // Earth Parameters + const int EARTHOFFSET = 284; + const int EARTHTOTALMIN = 24 * 60; + const int EARTHDAYSINYEAR = 365; + const double EARTHTILTAXIS = 23.44; // IN DEGREES + const double EARTHHOURANGLEFACTOR = 15; + + // Mars Parameters + const int MARSOFFSET = 250; // higher value use to model mars as earth + const int MARSTOTALMIN = 24 * 60 + 40; + const int MARSDAYSINYEAR = 687; + const double MARSTILTAXIS = 25.19; // IN DEGREE + const double MARSHOURANGLEFACTOR = 14.6; + + /* NOTE : Considering sky color of a planet as a function of solar altitude */ + + float EARTHNIGHT [4] = {0.0, 0.0, 0.0, 1.0}; + float EARTHNOON [4] = {0.5294, 0.8078, 0.9216, 1.0}; + float EARTHDUSKDAWN[4] = {0.546, 0.45, 0.3423, 1.0}; + + float *EARTHSKYCOLOR[3] = {&EARTHNIGHT[0], &EARTHDUSKDAWN[0], &EARTHNOON[0]}; + + float MARSNIGHT [4] = {0.02, 0.02, 0.05, 1.0}; + float MARSNOON [4] = {1, 0.412, 0.05, 1.0}; + float MARSDUSKDAWN[4] = {0.85, 0.45, 0.30, 1.0}; + + float *MARSSKYCOLOR[3] = {&MARSNIGHT[0], &MARSDUSKDAWN[0], &MARSNOON[0]}; + + float NIGHTTHRES = -0.3; + float DUSKDAWNTHRES = 0.15; // after this point there will be a major transition in sky color ; Dusk -> Noon && Noon -< Dawn + }; +} + +///////////////////////////////////////////////// +gz::sim::plugins::DayLightManager::DayLightManager() : GuiSystem(), + _dataPtr(new gz::sim::plugins::DayLightManagerPrivate), + _radius(150), // : distance of sun position from origin, default 150 + _timeOfDayMin(720), // : variable time (in min) which changes + _day(1), // : Day 1 of Gregorian calendar, current constant , TODO : make it variable, increase as time passes + _observerLatitude(28.6139), // : Default observer Latitude + _y_bias(0), // : shift of sun position in y direction + _x_bias(0), // : shift of sun position in x direction + _speedMultiplier(1), // : speed rate + _bgSet(false) // : store whether or not background color is set on sun transition + +{ + // Code essentials paramter intialize + _dataPtr->_init = false; + _dataPtr->_curntRate = 100; + _dataPtr->_updateRateCnt = 0; + _dataPtr->_gzMsgsTimeout = 1000; + + // Default planet parameter intialize + _dataPtr->_planetSelect = "Earth"; + _dataPtr->_totalMin = _dataPtr->EARTHTOTALMIN; + _dataPtr->_selectPlanet = _dataPtr->EARTHSKYCOLOR; + + this->_timeOfDayHour = (this->_timeOfDayMin * 1.0) / 60; + + this->_hourAngleFactor = this->_dataPtr->EARTHHOURANGLEFACTOR; + this->_hourAngle = this->_hourAngleFactor * (this->_timeOfDayHour - 12); + this->_hourAngleRadian = ToRadian(this->_hourAngle); + + this->_observerLatitudeRadian = ToRadian(this->_observerLatitude); + + this->_sunDeclination = _dataPtr->EARTHTILTAXIS * sin(2 * M_PI * (_dataPtr->EARTHOFFSET + this->_day) / _dataPtr->EARTHDAYSINYEAR); + this->_sunDeclinationRadian = ToRadian(this->_sunDeclination); + + this->R_next = (this->_dataPtr->_selectPlanet[2])[0]; + this->G_next = (this->_dataPtr->_selectPlanet[2])[1]; + this->B_next = (this->_dataPtr->_selectPlanet[2])[2]; + + // publish time on daylightmanager/time/sec topic + this->_dataPtr->_pub = this->_dataPtr->_node.Advertise("daylightmanager/time/sec"); + this->_dataPtr->_timeMsg.set_sec(this->_timeOfDayMin * 60); + this->_dataPtr->_pub.Publish(this->_dataPtr->_timeMsg); + + // Connect with gz GUI application + gz::gui::App()->findChild()->installEventFilter(this); +} + +///////////////////////////////////////////////// +gz::sim::plugins::DayLightManager::~DayLightManager() = default; + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::LoadConfig(const tinyxml2::XMLElement * /*_pluginElem*/) +{ + if (this->title.empty()) + this->title = "Day Light Manager"; +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::Update(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) +{ + if (!this->_dataPtr->_init) // one time: insert sun sphere and update its position + gz::sim::plugins::DayLightManager::Setup(_ecm); + + if (!_info.paused) + { + if (this->_dataPtr->_init) // update sun positions depending upon Latitude, Month, and Day + gz::sim::plugins::DayLightManager::SetSunPosition(); + + this->_dataPtr->_updateRateCnt++; + } +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::Setup(gz::sim::EntityComponentManager &_ecm) +{ + // Get the world name from ecm + _ecm.Each( + [&](const Entity &, + const components::World *, + const components::Name *_name) -> bool + { + this->_dataPtr->_worldName = _name->Data(); + return true; + }); + + // Remove pre-existing light source + std::string lightSource = "sun"; + gz::msgs::Entity sunRemoveEntity; + gz::msgs::Boolean sunRemoveRes; + bool sunRemoveResult; + sunRemoveEntity.set_name(lightSource); + sunRemoveEntity.set_type(gz::msgs::Entity::LIGHT); + std::string sunRemoveService = "/world/" + this->_dataPtr->_worldName + "/remove"; + this->_dataPtr->_node.Request(sunRemoveService, sunRemoveEntity, this->_dataPtr->_gzMsgsTimeout, sunRemoveRes, sunRemoveResult); + + // sun sphere sdf file + sdf::SDF sunSDF; + sunSDF.SetFromString( + R"( + + + + true + 0 0 0 0 0 0 + + + false + + + 2.0 + + + + 1 + 1 1 0.7 1 + 1 1 0.7 1 + 1 1 1 1 + 1 1 0.5 1 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 1 1 1 1 + + 100 + 0.9 + 0.01 + 0.001 + + 0 0 -1 + + + + + )"); + + // Insert sun_sphere model + gz::msgs::EntityFactory sunSphereSpawnEntity; + gz::msgs::Boolean sunSphereSpawnRes; + bool sunSpawnResult; + sunSphereSpawnEntity.set_sdf(sunSDF.ToString()); + sunSphereSpawnEntity.set_name(this->_dataPtr->_sunModelName); + std::string sunSphereSpawnService = "/world/" + this->_dataPtr->_worldName + "/create"; + this->_dataPtr->_node.Request(sunSphereSpawnService, sunSphereSpawnEntity, this->_dataPtr->_gzMsgsTimeout, sunSphereSpawnRes, sunSpawnResult); + + // Set initial sun position + gz::msgs::Vector3d *position = new gz::msgs::Vector3d; + position->set_x(0); + position->set_y(0); + position->set_z(this->_radius); + + this->_dataPtr->_sunSpherePose.set_name(this->_dataPtr->_sunModelName); + this->_dataPtr->_sunSpherePose.set_allocated_position(position); + + this->_dataPtr->_serviceName = "/world/" + this->_dataPtr->_worldName + "/set_pose"; + this->_dataPtr->_node.Request(this->_dataPtr->_serviceName, this->_dataPtr->_sunSpherePose, this->_dataPtr->_gzMsgsTimeout, this->_dataPtr->_sunSpherePoseRes, this->_dataPtr->_sunSpherePoseResult); + + this->_dataPtr->_init = true; +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::SetSunPosition() +{ + if (this->_dataPtr->_updateRateCnt >= this->_dataPtr->_curntRate) + { + + /* _References_ */ + // https://andrewmarsh.com/apps/staging/sunpath3d.html + // https://ntrs.nasa.gov/api/citations/20200003207/downloads/20200003207.pdf + // https://stackoverflow.com/questions/8708048/position-of-the-sun-given-time-of-day-latitude-and-longitude + // https://ntrs.nasa.gov/api/citations/19940010257/downloads/19940010257.pdf + + /* __Parameters needed to calculate sun position__ */ + // observer latitude (φ) + // sun's declination (δ) + // hour angle (ω) + // day (1/jan as 1 day) + + // δ = 23.44° * sin(2π * (offset + day) / no_of_days_in_year) + // θ = arctan2(sin(ω), (cos(ω) * sin(φ) - tan(δ) * cos(φ))) + // α = arcsin(sin(φ) * sin(δ) + cos(φ) * cos(δ) * cos(ω)) + + // convert polar to cartesian coordinates + // x = r * cos(θ) * cos(α) + // y = r * sin(θ) * cos(α) + // z = r * sin(α) + + // Calculate time of day and hour angle + this->_timeOfDayHour = (this->_timeOfDayMin * 1.0) / 60; + this->_hourAngle = this->_hourAngleFactor * (this->_timeOfDayHour - 12); + this->_hourAngleRadian = ToRadian(this->_hourAngle); + + // Calculate sun position + float theta = atan2(sin(this->_hourAngleRadian), (cos(this->_hourAngleRadian) * sin(this->_observerLatitudeRadian) - tan(this->_sunDeclinationRadian) * cos(this->_observerLatitudeRadian))); + float alpha = asin(sin(this->_observerLatitudeRadian) * sin(this->_sunDeclinationRadian) + cos(this->_observerLatitudeRadian) * cos(this->_sunDeclinationRadian) * cos(this->_hourAngleRadian)); + + // Update sun coordinates + this->_x_coordinate = this->_radius * cos(alpha) * cos(theta) + this->_x_bias; + this->_y_coordinate = this->_radius * cos(alpha) * -1* sin(theta) + this->_y_bias; + this->_z_coordinate = this->_radius * sin(alpha); + + // Set new sun position + gz::msgs::Vector3d *position = new gz::msgs::Vector3d; + position->set_x(this->_x_coordinate); + position->set_y(this->_y_coordinate); + position->set_z(this->_z_coordinate); + + double yaw = atan2(this->_y_coordinate, this->_x_coordinate); + double pitch = atan2(sqrt((pow(this->_x_coordinate, 2) + pow(this->_y_coordinate, 2))), this->_z_coordinate); + double roll = 0; + + gz::sim::plugins::DayLightManager::EulerToQuat(roll, pitch, yaw); + + gz::msgs::Quaternion *orientation = new gz::msgs::Quaternion; + orientation->set_w(this->_sunAngleRollQuat[0]); + orientation->set_x(this->_sunAngleRollQuat[1]); + orientation->set_y(this->_sunAngleRollQuat[2]); + orientation->set_z(this->_sunAngleRollQuat[3]); + + this->_dataPtr->_sunSpherePose.set_name(this->_dataPtr->_sunModelName); + this->_dataPtr->_sunSpherePose.set_allocated_position(position); + this->_dataPtr->_sunSpherePose.set_allocated_orientation(orientation); + + this->_dataPtr->_node.Request(this->_dataPtr->_serviceName, this->_dataPtr->_sunSpherePose, this->_dataPtr->_gzMsgsTimeout, this->_dataPtr->_sunSpherePoseRes, this->_dataPtr->_sunSpherePoseResult); + + // Update time display + std::string displayTime = std::to_string(int(this->_timeOfDayHour)) + " : " + std::to_string(int(this->_timeOfDayMin) % 60) + " Hours"; + this->time = QString::fromStdString(displayTime); + gz::sim::plugins::DayLightManager::timeChanged(); + + if (_bgSet) + gz::sim::plugins::DayLightManager::setBgColor(); + + this->_dataPtr->_updateSky = true; + + // publish data + this->_dataPtr->_timeMsg.set_sec(this->_timeOfDayMin * 60); + this->_dataPtr->_pub.Publish(this->_dataPtr->_timeMsg); + + // Increment time + this->_timeOfDayMin += 1; + this->_timeOfDayMin = this->_timeOfDayMin % this->_dataPtr->_totalMin; + + this->_dataPtr->_updateRateCnt = 0; + } +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::setBgColor() +{ + + float normalizeHeight = this->_z_coordinate / this->_radius; + + // Lambda function for smooth transition + auto sigmoid = [](float x, float smoothness = 1.0f) + { + return 1.0f / (1.0f + std::exp(-smoothness * x)); + }; + + // Adjust the night threshold + if (normalizeHeight <= this->_dataPtr->NIGHTTHRES) + { + // Full night + this->R_next = (this->_dataPtr->_selectPlanet[0])[0]; + this->G_next = (this->_dataPtr->_selectPlanet[0])[1]; + this->B_next = (this->_dataPtr->_selectPlanet[0])[2]; + } + + else + { + // Calculate transition factor + float nightToDuskFactor = std::clamp((normalizeHeight - this->_dataPtr->NIGHTTHRES) / (this->_dataPtr->DUSKDAWNTHRES - this->_dataPtr->NIGHTTHRES), 0.0f, 1.0f); + float t1 = sigmoid(nightToDuskFactor * 2 - 1, 4.0f); + + // Interpolate between night and dusk + float r1 = (this->_dataPtr->_selectPlanet[0])[0] * (1 - t1) + (this->_dataPtr->_selectPlanet[1])[0] * t1; + float g1 = (this->_dataPtr->_selectPlanet[0])[1] * (1 - t1) + (this->_dataPtr->_selectPlanet[1])[1] * t1; + float b1 = (this->_dataPtr->_selectPlanet[0])[2] * (1 - t1) + (this->_dataPtr->_selectPlanet[1])[2] * t1; + + if (normalizeHeight <= this->_dataPtr->DUSKDAWNTHRES) + { + // Between night and full dusk/dawn + this->R_next = r1; + this->G_next = g1; + this->B_next = b1; + } + else + { + // Transition from dusk to day + float duskToDayFactor = std::clamp((normalizeHeight - this->_dataPtr->DUSKDAWNTHRES) / (1 - this->_dataPtr->DUSKDAWNTHRES), 0.0f, 1.0f); + float t2 = sigmoid(duskToDayFactor * 2 - 1, 4.0f); + + // Interpolate between dusk and day + this->R_next = r1 * (1 - t2) + (this->_dataPtr->_selectPlanet[2])[0] * t2; + this->G_next = g1 * (1 - t2) + (this->_dataPtr->_selectPlanet[2])[1] * t2; + this->B_next = b1 * (1 - t2) + (this->_dataPtr->_selectPlanet[2])[2] * t2; + } + } +} + +///////////////////////////////////////////////// +float gz::sim::plugins::DayLightManager::ToRadian(float degree) +{ + return (degree) * (M_PI / 180); +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::SetPlanet(const QString &_planet) +{ + this->_dataPtr->_planetSelect = _planet.toStdString(); + + if (this->_dataPtr->_planetSelect == "Earth") + { + + this->_day = 1; + this->_sunDeclination = this->_dataPtr->EARTHTILTAXIS * sin(2 * M_PI * (this->_dataPtr->EARTHOFFSET + this->_day) / this->_dataPtr->EARTHDAYSINYEAR); + this->_sunDeclinationRadian = ToRadian(this->_sunDeclination); + + this->_timeOfDayMin = 0; + this->_hourAngleFactor = this->_dataPtr->EARTHHOURANGLEFACTOR; + this->_dataPtr->_totalMin = _dataPtr->EARTHTOTALMIN; + _dataPtr->_selectPlanet = _dataPtr->EARTHSKYCOLOR; + } + + if (this->_dataPtr->_planetSelect == "Mars") + { + + this->_day = 1; + this->_sunDeclination = this->_dataPtr->MARSTILTAXIS * sin(2 * M_PI * (this->_dataPtr->MARSOFFSET + this->_day) / this->_dataPtr->MARSDAYSINYEAR); + this->_sunDeclinationRadian = ToRadian(this->_sunDeclination); + + this->_timeOfDayMin = 0; + this->_hourAngleFactor = this->_dataPtr->MARSHOURANGLEFACTOR; + this->_dataPtr->_totalMin = _dataPtr->MARSTOTALMIN; + _dataPtr->_selectPlanet = _dataPtr->MARSSKYCOLOR; + } +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::SetLatitude(double _latitude) +{ + this->_observerLatitude = _latitude; + this->_observerLatitudeRadian = ToRadian(this->_observerLatitude); +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::SetTime(int _timeOfDay) +{ + this->_timeOfDayMin = _timeOfDay * 60; +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::SetSpeed(int _speedMultiplier) +{ + this->_speedMultiplier = _speedMultiplier; + this->_dataPtr->_curntRate = this->_dataPtr->_updateRate / this->_speedMultiplier; + this->_dataPtr->_updateRateCnt = 0; +} + +void gz::sim::plugins::DayLightManager::SetRadius(int _radius) +{ + this->_radius = _radius; +} + +void gz::sim::plugins::DayLightManager::SetX(int _x_bias) +{ + + this->_x_bias = _x_bias; +} + +void gz::sim::plugins::DayLightManager::SetY(int _y_bias) +{ + + this->_y_bias = _y_bias; +} + +void gz::sim::plugins::DayLightManager::BgColor(bool _checked) +{ + this->_bgSet = _checked; +} +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::EulerToQuat(float roll, float pitch, float yaw) +{ + float cy = cos(yaw * 0.5); + float sy = sin(yaw * 0.5); + float cp = cos(pitch * 0.5); + float sp = sin(pitch * 0.5); + float cr = cos(roll * 0.5); + float sr = sin(roll * 0.5); + + float w = cr * cp * cy + sr * sp * sy; + float x = sr * cp * cy - cr * sp * sy; + float y = cr * sp * cy + sr * cp * sy; + float z = cr * cp * sy - sr * sp * cy; + + // Normalize the quaternion + float magnitude = std::sqrt(w * w + x * x + y * y + z * z); + if (magnitude != 0.0f) + { + this->_sunAngleRollQuat[0] = w / magnitude; + this->_sunAngleRollQuat[1] = x / magnitude; + this->_sunAngleRollQuat[2] = y / magnitude; + this->_sunAngleRollQuat[3] = z / magnitude; + } + else + { + // Handle error case (division by zero) + this->_sunAngleRollQuat[0] = 1.0; + this->_sunAngleRollQuat[1] = 0.0; + this->_sunAngleRollQuat[2] = 0.0; + this->_sunAngleRollQuat[3] = 0.0; + } +} + +///////////////////////////////////////////////// +bool gz::sim::plugins::DayLightManager::eventFilter(QObject *_obj, QEvent *_event) +{ + + if (_event->type() == gz::gui::events::Render::kType) + if (this->_dataPtr->_updateSky) + gz::sim::plugins::DayLightManager::PerformRenderingOperations(); + + return QObject::eventFilter(_obj, _event); +} + +void gz::sim::plugins::DayLightManager::PerformRenderingOperations() +{ + + if (nullptr == this->scene) + gz::sim::plugins::DayLightManager::FindScene(); + + if (nullptr == this->scene) + return; + + if (_bgSet) + { + this->scene->SetBackgroundColor({this->R_next, + this->G_next, + this->B_next, + 1.0}); + + this->_dataPtr->_updateSky = false; + } +} + +///////////////////////////////////////////////// +void gz::sim::plugins::DayLightManager::FindScene() +{ + + auto loadedEngNames = gz::rendering::loadedEngines(); + + if (loadedEngNames.empty()) + { + gzdbg << "No rendering engine is loaded yet" << std::endl; + return; + } + + auto engineName = loadedEngNames[0]; + if (loadedEngNames.size() > 1) + { + gzdbg << "More than one engine is available. " + << "Using engine [" << engineName << "]" << std::endl; + } + + auto engine = gz::rendering::engine(engineName); + if (!engine) + { + gzerr << "Internal error: failed to load engine [" << engineName + << "]. Grid plugin won't work." << std::endl; + return; + } + + if (engine->SceneCount() == 0) + { + gzdbg << "No scene has been created yet" << std::endl; + return; + } + + auto scenePtr = engine->SceneByIndex(0); + if (nullptr == scenePtr) + { + gzerr << "Internal error: scene is null." << std::endl; + return; + } + + if (engine->SceneCount() > 1) + gzdbg << "More than one scene is available. " << "Using scene [" << scene->Name() << "]" << std::endl; + + if (!scenePtr->IsInitialized() || nullptr == scenePtr->RootVisual()) + return; + + this->scene = scenePtr; +} + +///////////////////////////////////////////////// +GZ_ADD_PLUGIN(gz::sim::plugins::DayLightManager, + gz::gui::Plugin) \ No newline at end of file diff --git a/custom_gz_plugins/src/DayLightManager/DayLightManager.hh b/custom_gz_plugins/src/DayLightManager/DayLightManager.hh new file mode 100644 index 00000000..1f9341b3 --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/DayLightManager.hh @@ -0,0 +1,214 @@ +/* + * Jayesh Chaudhary + * + * 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 GZ_GUI_DAYLIGHTMANAGER_PLUGINS_HH_ +#define GZ_GUI_DAYLIGHTMANAGER_PLUGINS_HH_ + +#include + +#include +#include + +#include +#include + +namespace gz::sim::plugins { + +class DayLightManagerPrivate; + +/// \brief A class to manage daylight simulation in a GUI environment +class DayLightManager : public gz::sim::GuiSystem +{ + Q_OBJECT + +public: + /// \brief Constructor for DayLightManager + DayLightManager(); + + /// \brief Destructor for DayLightManager + ~DayLightManager() override; + + /// \brief Loads configuration from an XML element + /// \param[in] _pluginElem XML element containing configuration data + void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Updates the DayLightManager state + /// \param[in] _info Update information + /// \param[in] _ecm Entity component manager + void Update(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Sets up the DayLightManager + /// \param[in] _ecm Entity component manager + void Setup(gz::sim::EntityComponentManager &_ecm); + + /// \brief Sets the sun position based on current time and location + void SetSunPosition(); + + /// \brief Converts Euler angles to a quaternion + /// \param[in] roll Roll angle in radians + /// \param[in] pitch Pitch angle in radians + /// \param[in] yaw Yaw angle in radians + void EulerToQuat(float roll, float pitch, float yaw); + +public slots: + /// \brief Sets the planet for daylight calculations + /// \param[in] _planet Name of the planet + void SetPlanet(const QString & _planet); + + /// \brief Sets the latitude for daylight calculations + /// \param[in] _latitude Latitude in degrees + void SetLatitude(double _latitude); + + /// \brief Sets the time of day + /// \param[in] _timeOfDay Time of day in minutes since midnight + void SetTime(int _timeOfDay); + + /// \brief Sets the speed multiplier for time progression + /// \param[in] _speedMultiplier Speed multiplier for time progression + void SetSpeed(int _speedMultiplier); + + /// \brief Sets the radius for solar path + /// \param[in] _radius radius for solar path + void SetRadius(int _radius); + + /// \brief Sets X coordinate of sun + /// \param[in] _x_bias bias value to shift sun x coordinate + void SetX(int _x_bias); + + /// \brief Sets Y coordinate of sun + /// \param[in] _y_bias bias value to shift sun y coordinate + void SetY(int _y_bias); + + /// \brief Check if user set background transition or not + /// \param[in] e + void BgColor(bool _checked ); + +private: + /// \brief Pointer to private data + std::unique_ptr _dataPtr; + +public: + /// \brief Converts degrees to radians + /// \param[in] radian Angle in degrees + /// \return Angle in radians + float ToRadian(float radian); + + /// \brief Property for current time display + Q_PROPERTY(QString time MEMBER time NOTIFY timeChanged) + +signals: + /// \brief Signal emitted when time changes + void timeChanged(); + +public: + /// \brief Current time display string + QString time {QString::fromStdString("12 : 00 Hours")}; + +private: + /// \brief Pointer to the rendering scene + gz::rendering::ScenePtr scene{nullptr}; + + /// \brief Perform rendering operations + void PerformRenderingOperations(); + + /// \brief Event filter for Qt events + /// \param[in] _obj Object that triggered the event + /// \param[in] _event The event that occurred + /// \return True if the event was handled, false otherwise + bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Find the rendering scene + void FindScene(); + +public: + /// \brief Set the background color + void setBgColor(); + +public: + + /// \brief distance of sun from origin + int _radius; + + /// \brief Time of day in minutes + int _timeOfDayMin; + + /// \brief Current day + int _day; + + /// \brief Time of day in hours + float _timeOfDayHour; + + /// \brief Factor for hour angle calculation + double _hourAngleFactor; + + /// \brief Hour angle + float _hourAngle; + + /// \brief Hour angle in radians + float _hourAngleRadian; + + /// \brief Observer's latitude + double _observerLatitude; + + /// \brief Sun's declination + float _sunDeclination; + + /// \brief Sun's declination in radians + float _sunDeclinationRadian; + + /// \brief Observer's latitude in radians + float _observerLatitudeRadian; + + /// \brief X coordinate of the sun + float _x_coordinate; + + /// \brief Y coordinate of the sun + float _y_coordinate; + + /// \brief Z coordinate of the sun + float _z_coordinate; + + /// \brief Y bias for sun position + int _y_bias; + + /// \brief X bias for sun position + int _x_bias; + + /// \brief Next R value for color transition + float R_next; + + /// \brief Next G value for color transition + float G_next; + + /// \brief Next B value for color transition + float B_next; + + /// \brief Sun angle roll quaternion + float _sunAngleRollQuat[4]; + + /// \brief Speed multiplier for time progression + float _speedMultiplier; + + /// \brief Speed multiplier for time progression + bool _bgSet; + + +}; + +} + +#endif \ No newline at end of file diff --git a/custom_gz_plugins/src/DayLightManager/DayLightManager.qml b/custom_gz_plugins/src/DayLightManager/DayLightManager.qml new file mode 100644 index 00000000..cd95bb50 --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/DayLightManager.qml @@ -0,0 +1,213 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.2 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 4 + columnSpacing: 10 + rowSpacing: 5 // Reduced from 10 + Layout.minimumWidth: 450 + Layout.minimumHeight: 450 + anchors.fill: parent + anchors.margins: 10 + + // Live Time + ColumnLayout { + id: liveTime + Layout.columnSpan: 4 + Layout.fillWidth: true + Layout.alignment: Qt.AlignCenter + + Label { + text: DayLightManager.time + font.pixelSize: 16 + font.bold: true + Layout.alignment: Qt.AlignCenter + } + } + + // Planet Selection + Label { + text: "Planet:" + font.pixelSize: 14 + } + + ComboBox { + id: planetComboBoxId + Layout.fillWidth: true + Layout.columnSpan: 3 + model: ["Earth","Mars"] + currentIndex: 0 + onCurrentIndexChanged: { + DayLightManager.SetPlanet(model[currentIndex]); + } + + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + ToolTip.text: qsTr("Select the planet to view the day light") + } + + // Latitude + Label { + text: "Latitude:" + font.pixelSize: 14 + } + + TextField { + id: latitudeId + Layout.fillWidth: true + Layout.columnSpan: 3 + text: "28.6139" + validator: DoubleValidator { + bottom: -90 + top: 90 + decimals: 2 + notation: DoubleValidator.StandardNotation + } + selectByMouse: true + + property string errorMessage: "" + + ToolTip.visible: hovered || errorMessage !== "" + ToolTip.delay: hovered ? Qt.styleHints.mousePressAndHoldInterval : 0 + ToolTip.text: errorMessage !== "" ? errorMessage : qsTr("Enter the latitude of the location") + + onTextChanged: { + var value = parseFloat(text) + if (isNaN(value) || value < -90 || value > 90) { + errorMessage = qsTr("Invalid latitude! Must be between -90 and 90.") + color = "red" + } else { + errorMessage = "" + color = "black" + DayLightManager.SetLatitude(value); + } + } + } + + // Set Time + Label { + text: "Set Time:" + font.pixelSize: 14 + } + + TextField { + id: timeTextId + Layout.fillWidth: true + Layout.columnSpan: 3 + text: "12" + validator: IntValidator { + bottom: 0 + top: 24 + } + selectByMouse: true + + property string errorMessage: "" + + ToolTip.visible: hovered || errorMessage !== "" + ToolTip.delay: hovered ? Qt.styleHints.mousePressAndHoldInterval : 0 + ToolTip.text: errorMessage !== "" ? errorMessage : qsTr("Enter time (0-24) hours") + + onTextChanged: { + var value = parseInt(text) + if (isNaN(value) || value < 0 || value > 24) { + errorMessage = qsTr("Invalid Time of Day ! Must be between 0 and 24.") + color = "red" + } else { + errorMessage = "" + color = "black" + DayLightManager.SetTime(value); + } + } + } + + // Speed + Label { + text: "Speed:" + font.pixelSize: 14 + } + + GzSpinBox { + id: speedSpinId + Layout.fillWidth: true + Layout.columnSpan: 3 + value: 1 + minimumValue: 1 + maximumValue: 100 + stepSize: 1 + onEditingFinished: { + DayLightManager.SetSpeed(speedSpinId.value); + } + } + + // X and Y coordinates + Label { + text: "X:" + font.pixelSize: 14 + } + + GzSpinBox { + id: xSpinId + value: 0 + minimumValue: -10000 + maximumValue: 10000 + stepSize: 1 + onEditingFinished: { + DayLightManager.SetX(xSpinId.value); + } + } + + Label { + text: "Y:" + font.pixelSize: 14 + Layout.alignment: Qt.AlignRight | Qt.AlignVCenter // Align to the center vertically +} + +GzSpinBox { + id: ySpinId + value: 0 + minimumValue: -1000 + maximumValue: 10000 + stepSize: 1 + Layout.alignment: Qt.AlignLeft | Qt.AlignVCenter // Align to the center vertically + onEditingFinished: { + DayLightManager.SetY(ySpinId.value); + } +} + + // Radius + Label { + text: "Radius:" + font.pixelSize: 14 + } + + GzSpinBox { + id: radiusSpinId + value: 100 + minimumValue: 1 + maximumValue: 1000000 + stepSize: 1 + onEditingFinished: { + DayLightManager.SetRadius(radiusSpinId.value); + } + + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + ToolTip.text: qsTr("Enter the solar path radius") + } + + CheckBox { + id: autoTimeProgressionId + text: "Background Set" + checked: false + onCheckedChanged: { + DayLightManager.BgColor(checked); + } + + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + ToolTip.text: qsTr("Enable or disable background set") + } + +} \ No newline at end of file diff --git a/custom_gz_plugins/src/DayLightManager/DayLightManager.qrc b/custom_gz_plugins/src/DayLightManager/DayLightManager.qrc new file mode 100644 index 00000000..0aef4a17 --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/DayLightManager.qrc @@ -0,0 +1,5 @@ + + + DayLightManager.qml + + \ No newline at end of file diff --git a/custom_gz_plugins/src/DayLightManager/README.md b/custom_gz_plugins/src/DayLightManager/README.md new file mode 100644 index 00000000..5fa5ae30 --- /dev/null +++ b/custom_gz_plugins/src/DayLightManager/README.md @@ -0,0 +1,48 @@ +# Daylight Manager Plugin + +The Sun plays a pivotal role in driving most planetary phenomena, influencing nearly every environmental aspect, directly or indirectly. Accurately modeling the Sun's path is essential for realistic environmental simulations, especially in complex scenarios. + +The Daylight Manager plugin for Gazebo simulates the Sun's trajectory based on the time of day and the observer's latitude, providing a realistic and dynamic representation of sunlight. The shadow of object changes depending on the Sun's position, as depicted below + +![Sun Trajectory Simulation](assets/day_light_manager_transition.gif) + +# Features: +1. Sun Trajectory Variation: + + - **Time of Day**: The Sun’s altitude changes throughout the day, and users can set a custom time to observe these variations. + - **Latitude**: The observer's latitude influences solar altitude, and users can customize the latitude to adjust the Sun’s path accordingly. Below is an gif of the Sun's trajectory at -80° latitude on Earth. ( Right gif reference : https://andrewmarsh.com/apps/staging/sunpath3d.html ) + +![Sun Trajectory Simulation Latitude](assets/day_light_manager_transition_latitude_merge.gif) + + +2. **Live Time Update**: Real-time updates are displayed in the GUI. + +3. **Adjustable Sun Position**: Users can modify the Sun's position with x and y offsets. + +4. **Dynamic Lighting**: As the Sun's altitude changes, the simulation's background color dynamically adjusts to reflect different lighting conditions. + +5. **Multi-Planet Support**: The plugin supports Sun trajectories for both Earth and Mars, allowing users to switch between them. + +6. **Adjustable Rotation Radius**: The radius of the Sun's rotation can be modified using a radius spin box. + +7. **Simulation Speed Control**: The Sun trajectory simulation speed can be accelerated up to 100x. + + +# GUI +The GUI provides an interface to control the scene the sun's trajectory. +- Select Planet +- Configure latitude +- Set current time +- Change time speed constant +- X and Y origin offset +- Radius of distance from the origin to the sun +- Chekbox to change the background scene + +Sun Trajectory GUI + + +### Refs : +1. https://andrewmarsh.com/apps/staging/sunpath3d.html +2. https://ntrs.nasa.gov/api/citations/20200003207/downloads/20200003207.pdf +3. https://stackoverflow.com/questions/8708048/position-of-the-sun-given-time-of-day-latitude-and-longitude +4. https://ntrs.nasa.gov/api/citations/19940010257/downloads/19940010257.pdf diff --git a/custom_gz_plugins/src/DayLightManager/assets/GUI.png b/custom_gz_plugins/src/DayLightManager/assets/GUI.png new file mode 100644 index 00000000..f010fffd Binary files /dev/null and b/custom_gz_plugins/src/DayLightManager/assets/GUI.png differ diff --git a/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition.gif b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition.gif new file mode 100644 index 00000000..25e81f1a Binary files /dev/null and b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition.gif differ diff --git a/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude.gif b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude.gif new file mode 100644 index 00000000..1970bf40 Binary files /dev/null and b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude.gif differ diff --git a/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude_merge.gif b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude_merge.gif new file mode 100644 index 00000000..d277354f Binary files /dev/null and b/custom_gz_plugins/src/DayLightManager/assets/day_light_manager_transition_latitude_merge.gif differ diff --git a/custom_gz_plugins/src/DroneDustPlugin/CMakeLists.txt b/custom_gz_plugins/src/DroneDustPlugin/CMakeLists.txt new file mode 100644 index 00000000..c8faa05e --- /dev/null +++ b/custom_gz_plugins/src/DroneDustPlugin/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(gz-cmake3 REQUIRED) + +project(DroneDust) + +find_package(gz-plugin2 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + +find_package(gz-sim8 REQUIRED) +find_package(GTest REQUIRED) + +add_library(DroneDust SHARED DroneDust.cc ) +set_property(TARGET DroneDust PROPERTY CXX_STANDARD 17) +target_link_libraries(DroneDust + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim8::gz-sim8) + +install(TARGETS DroneDust + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) \ No newline at end of file diff --git a/custom_gz_plugins/src/DroneDustPlugin/DroneDust.cc b/custom_gz_plugins/src/DroneDustPlugin/DroneDust.cc new file mode 100644 index 00000000..b78dd24c --- /dev/null +++ b/custom_gz_plugins/src/DroneDustPlugin/DroneDust.cc @@ -0,0 +1,149 @@ +/* +The `DroneDustPrivate` is the main script which create a ignition gazebo node in oder to handle communication between the camera topics. +A subsciber is created which subscribes to the `/model/car/cmd_vel` topic build published by the camera plugin in the gazebo world. +The image recieved is first converted into opencv format using the GzCVBridge script on top of which a custom post processing +algorithm is applied using openCV in order to add the rain effect noise on the camera the frame. The rain augmented frame in them +published on a seperate topic called `/model/car/link/dust_link/particle_emitter/emitter/cmd`. +*/ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "DroneDust.hh" + + +namespace drone_dust +{ + class DroneDustPrivate + { + public: gz::transport::Node node; + + public: gz::transport::Node velDustNode; + public: gz::transport::Node::Publisher dustPub; + + public: std::string robotName; + public: std::string modelName; + + public: double actDist, minDist, densityConst, last_distance = 0; + public: double robot_x = 0, robot_y = 0, robot_z = 0; + public: double world_x = 0, world_y = 0, world_z = 0; + public: double scaleConst = 0.01; + public: double rateConst = 0.04; + + public: void publish_dust(double rate_calculated, double scale_rate_calculated); + public: void PoseCb(const gz::msgs::Pose_V &_data); + }; + + void DroneDustPrivate::PoseCb(const gz::msgs::Pose_V &_data) + { + for (int i = 0; i < _data.pose().size(); i++) { + // std::cout<<"Pose Name: "<< pose_name << std::endl; + if ( _data.pose(i).name() == robotName) + { + robot_x = _data.pose(i).position().x(); + robot_y = _data.pose(i).position().y(); + robot_z = _data.pose(i).position().z(); + // std::cout << "Robot " << robotName << " Z: " << robot_z << " index: " << i << std::endl; + } + } + for (int i = 0; i < _data.pose().size(); i++) { + std::string pose_name = _data.pose(i).name(); + if (pose_name.find(modelName) != std::string::npos) + { + world_x = _data.pose(i).position().x(); + world_y = _data.pose(i).position().y(); + world_z = _data.pose(i).position().z(); + // std::cout << "World Z: " << world_z << " index: " << i << std::endl; + } + } + double object_distance = robot_z - world_z; + gzmsg << "Object Distance: " << object_distance << std::endl; + + if(object_distance < actDist && object_distance > minDist) + { + // std::cout << "World Z: " << world_z << std::endl; + double rate = densityConst * (actDist/object_distance) * this->rateConst; + double scale_rate = densityConst * (actDist/object_distance) * this->scaleConst; + // std::cout << "Scale Rate " << scale_rate << std::endl; + this->publish_dust(rate, scale_rate); + // gzmsg << "Rate: " << rate << " " << "Scale Rate: " << scale_rate << std::endl; + } + else + { + this->publish_dust(0, 0); + } + last_distance = object_distance; + } + + void DroneDustPrivate::publish_dust(double rate_calculated, double scale_rate_calculated) + { + + gz::msgs::ParticleEmitter *msg = new gz::msgs::ParticleEmitter(); + gz::msgs::Float rate, scale_rate; + + rate.set_data(rate_calculated); + scale_rate.set_data(scale_rate_calculated); + + msg->set_allocated_rate(&rate); + msg->set_allocated_scale_rate(&scale_rate); + + this->dustPub.Publish(*msg); + + } + + DroneDust::DroneDust() + : dataPtr(std::make_unique()) + {} + + DroneDust::~DroneDust() + {} + + void DroneDust::Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &) + { + + auto worldName = _ecm.Component(_entity); + + std::string poseInfoTopic = "/world/" + worldName->Data() + "/pose/info"; + + if(_sdf->HasElement("activation_distance")) + this->dataPtr->actDist = _sdf->Get("activation_distance"); + if(_sdf->HasElement("min_distance")) + this->dataPtr->minDist = _sdf->Get("min_distance"); + if(_sdf->HasElement("density_constant")) + this->dataPtr->densityConst = _sdf->Get("density_constant"); + if(_sdf->HasElement("robot_name")) + this->dataPtr->robotName = _sdf->Get("robot_name"); + if(_sdf->HasElement("model_name")) + this->dataPtr->modelName = _sdf->Get("model_name"); + + std::string emitterTopic = "/model/" + this->dataPtr->robotName +"/link/dust_link/particle_emitter/emitter/cmd"; + + std::cout<< "Pose Topic: " << poseInfoTopic << std::endl; + std::cout<< "Emitter Topic: " << emitterTopic << std::endl; + std::cout<< "robot_name: " << this->dataPtr->robotName << std::endl; + std::cout<< "model_name: " << this->dataPtr->modelName << std::endl; + + std::string poseTopicProcessed = gz::transport::TopicUtils::AsValidTopic(poseInfoTopic); + std::string poseInfoTopicProcessed = gz::transport::TopicUtils::AsValidTopic(poseInfoTopic); + + this->dataPtr->node.Subscribe(poseInfoTopicProcessed, &DroneDustPrivate::PoseCb, this->dataPtr.get()); + + this->dataPtr->dustPub = this->dataPtr->velDustNode.Advertise(emitterTopic); + } +} + + GZ_ADD_PLUGIN( + drone_dust::DroneDust, + gz::sim::System, + drone_dust::DroneDust::ISystemConfigure + ) \ No newline at end of file diff --git a/custom_gz_plugins/src/DroneDustPlugin/DroneDust.hh b/custom_gz_plugins/src/DroneDustPlugin/DroneDust.hh new file mode 100644 index 00000000..f20cbb00 --- /dev/null +++ b/custom_gz_plugins/src/DroneDustPlugin/DroneDust.hh @@ -0,0 +1,34 @@ +#include +#include +#include +#include + +#define SCALE_CONST 0.9 +#define RATE_CONST 4.0 + +// used if rain is set as high or low +// random int between these can be used if rain is medium +#define MAX_VEL 5.0 + +// rain drop maturity means that as the drop falls it gathers more water and falls faster and becomes more blurry +namespace drone_dust +{ + class DroneDustPrivate; + + class DroneDust: + public gz::sim::System, + public gz::sim::ISystemConfigure +{ + public: DroneDust(); + + public: ~DroneDust() override; + + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + private: std::unique_ptr dataPtr; + +}; +} \ No newline at end of file diff --git a/custom_gz_plugins/src/DroneDustPlugin/README.md b/custom_gz_plugins/src/DroneDustPlugin/README.md new file mode 100644 index 00000000..cc4acc42 --- /dev/null +++ b/custom_gz_plugins/src/DroneDustPlugin/README.md @@ -0,0 +1,32 @@ +# Drone Dust Plugin + +The Drone Dust plugin not only simulates the dust clouds generated by a drone’s thrusters during takeoff and landing, but also replicates how these dust particles affect onboard sensors crucial for autonomous operations. + +- Depth Cameras: The dust clouds can scatter infrared or laser signals, causing depth cameras to misinterpret distances or fail to detect objects entirely, which can compromise altitude control and obstacle avoidance. + +- RGB Cameras: Dust particles obscure visibility, reducing image clarity and potentially hindering visual navigation, object detection, and scene recognition used for autonomous decisions. + +- 3D LiDAR: Dust clouds interfere with LiDAR by scattering its laser pulses, resulting in inaccurate 3D mapping and unreliable data on nearby objects or terrain, impacting the drone's ability to navigate and avoid obstacles autonomously. + +By simulating these dust interactions, the plugin provides a realistic testing environment for drones, ensuring that autonomous systems can adapt to harsh planetary conditions where dust is a significant factor. + +## Usage + +```xml + + 5 + 0.27 + 12.0 + ground_plane + ingenuity + +``` + +- In order to load the pugin, add the above plugin tag into the world sdf and configure the following tags: + - robot_name: Name of the robot in the simulation + - model_name: Name of the base model in the simulation (Dusty Model) + - density_const: Multiplier for the amount of dust to be produced + - min_distance: Minimum difference in height at which dust starts to produce + - activation_distance: Maximum difference in height at which dust stops to produce \ No newline at end of file diff --git a/custom_gz_plugins/src/DustManager/CMakeLists.txt b/custom_gz_plugins/src/DustManager/CMakeLists.txt new file mode 100644 index 00000000..672d870e --- /dev/null +++ b/custom_gz_plugins/src/DustManager/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +if(POLICY CMP0100) + cmake_policy(SET CMP0100 NEW) +endif() + +project(DustManager) + +set(CMAKE_AUTOMOC ON) + +find_package(gz-sim8 REQUIRED COMPONENTS gui) + +QT5_ADD_RESOURCES(resources_RCC ${PROJECT_NAME}.qrc) + +add_library(${PROJECT_NAME} SHARED + ${PROJECT_NAME}.cc + ${resources_RCC} +) +target_link_libraries(${PROJECT_NAME} + PRIVATE gz-sim8::gui +) + +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) \ No newline at end of file diff --git a/custom_gz_plugins/src/DustManager/DustManager.cc b/custom_gz_plugins/src/DustManager/DustManager.cc new file mode 100644 index 00000000..e7a0f347 --- /dev/null +++ b/custom_gz_plugins/src/DustManager/DustManager.cc @@ -0,0 +1,495 @@ +/* + * Copyright (C) 2024 Naman Malik namanmalik0210@gmail.com + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "gz/gui/Plugin.hh" + +#include + +#include +#include +#include +#include +#include + +#include "DustManager.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +namespace dust_manager +{ + +class DustManagerPrivate +{ + + /// \brief Transport node + public: gz::transport::Node node; + + /// \brief Dust Emitter publisher + public: gz::transport::Node::Publisher dustPublisher; + + /// \brief Wind publisher + public: gz::transport::Node::Publisher windPublisher; + + /// \brief Connection to the pre-render event + public: common::ConnectionPtr sceneUpdateConn{nullptr}; + + public: rendering::ParticleEmitterPtr emitterCurrentPtr{nullptr}; + + /// \brief Pointer to EventManager + public: EventManager *eventMgr{nullptr}; + + /// \brief Map of dust models + std::unordered_map emitterMap; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr _scene{nullptr}; + + /// \brief Visual whose color will be changed. + public: rendering::VisualPtr ledVisual{nullptr}; + + /// \brief Protect variables changed from transport and the user + public: std::recursive_mutex mutex; + + /// \brief Name of topic for Wind Topic + public: std::string windTopic{""}; + + /// \brief List of dust topics publishing + public: QStringList dustModelList; + + /// \brief Name of the world + public: std::string worldName{""}; + + /// \brief Wind Msg Pointer + public: gz::msgs::Wind *windMsg = new gz::msgs::Wind(); +}; + +///////////////////////////////////////////////// +DustManager::DustManager(): dataPtr(std::make_unique()) +{ + +} +///////////////////////////////////////////////// +DustManager::~DustManager() = default; + +///////////////////////////////////////////////// +void DustManager::LoadConfig(const tinyxml2::XMLElement * /*_pluginElem*/) +{ + if (this->title.empty()) + { + this->title = "Dust Control"; + } + gz::gui::App()->findChild()->installEventFilter(this); +} + +bool DustManager::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + // This event is called in the render thread, so it's safe to make + // rendering calls here + if (nullptr != this->dataPtr->_scene) + { + if (this->changeTexture) + { + std::shared_ptr imagePtr = std::make_shared(this->image); + std::cout << "Setting Texture 2" <material->SetTexture("New Image", imagePtr); // Throwing error here + // this->material->SetTexture("/test_image.png"); // Simulation freezes here + std::cout << "Changing Material" << std::endl; + this->dataPtr->emitterCurrentPtr->SetMaterial(this->material); + this->changeTexture = false; + } + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void DustManager::Update(const gz::sim::UpdateInfo & /*_info*/, + gz::sim::EntityComponentManager &_ecm) +{ + if (!this->dataPtr->_scene) + { + this->dataPtr->_scene = rendering::sceneFromFirstRenderEngine(); + } + // Get the name of the world + if (this->dataPtr->worldName.empty()) + { + _ecm.Each( + [&](const Entity &, + const components::World *, + const components::Name *_name) -> bool + { + this->dataPtr->worldName = _name->Data(); + return false; + }); + SetupWindTopic(); + } + + _ecm.Each( + [&](const Entity &, + const components::ParticleEmitter *, + const components::Name *) -> bool + { + FindEntityVisual(_ecm); + return true; + }); +} + +///////////////////////////////////////////////// +void DustManager::SetdustModelList(const QStringList &_dustModelList) +{ + this->dataPtr->dustModelList = _dustModelList; +} + +/////////////////////////////////////////////////// +QStringList DustManager::dustModelList() const +{ + return this->dataPtr->dustModelList; +} + +////////////////////////////////////////////////// +void DustManager::FindEntityVisual(gz::sim::EntityComponentManager &_ecm) +{ + for (unsigned int i = 0; i < this->dataPtr->_scene->VisualCount(); ++i) + { + gz::rendering::VisualPtr visual = this->dataPtr->_scene->VisualByIndex(i); + rendering::ParticleEmitterPtr emitterPtr = + std::dynamic_pointer_cast(visual); + + if (emitterPtr) + { + bool found = false; + for (const auto &pair : this->dataPtr->emitterMap) { + if (pair.second == emitterPtr) { + found = true; + break; + } + } + if (!found) + { + auto parentVisual = visual->Parent(); + gz::sim::Entity linkEntity = gz::sim::kNullEntity; + + if (parentVisual) + { + // Use parentVisual to get the associated link or model entity + auto userDataOpt = parentVisual->UserData("gazebo-entity"); + + if (std::holds_alternative(userDataOpt)) + { + linkEntity = static_cast(std::get(userDataOpt)); + } + } + if (linkEntity != gz::sim::kNullEntity) + { + gz::sim::Entity modelEntity = _ecm.ParentEntity(linkEntity); + + if (modelEntity != gz::sim::kNullEntity) + { + const auto *modelName = _ecm.Component(modelEntity); + if (modelName) + { + const std::string& modelNameStr = modelName->Data(); + this->dataPtr->emitterMap[modelNameStr] = emitterPtr; + this->dataPtr->dustModelList.push_back(QString::fromStdString(modelNameStr)); + ConfigureEmitter(modelNameStr); + this->dustModelListChanged(); + } + } + } + } + } + } +} + + +///////////////////////////////////////////////// +void DustManager::SetupWindTopic() +{ + std::string windTopic = "/world/" + this->dataPtr->worldName + "/wind"; + this->dataPtr->windPublisher = this->dataPtr->node.Advertise(windTopic); +} + +///////////////////////////////////////////////// +void DustManager::OnDustEmitterModel(const QString &_DustEmitterModel) +{ + // Mutex Guard + std::lock_guard lock(this->dataPtr->mutex); + + std::string emitter_name = _DustEmitterModel.toStdString(); + + if (!emitter_name.empty()) + { + ConfigureEmitter(emitter_name); + } +} + +///////////////////////////////////////////////// +double DustManager::GetDensity(double _lifetime, double _rate, double _scalRate, gz::math::Vector3d _size) +{ + double density = _lifetime * _rate * _scalRate / (_size.X() * _size.Y() * _size.Z()); + return density; +} + +///////////////////////////////////////////////// +QColor GzColorToQColor(const gz::math::v7::Color &gzColor) +{ + int red = static_cast(gzColor.R() * 255); + int green = static_cast(gzColor.G() * 255); + int blue = static_cast(gzColor.B() * 255); + int alpha = static_cast(gzColor.A() * 255); + + gzmsg << "red: "<< red << " " << "green: "<< green << " " << "blue: "<< blue << " " << "Alpha: "<< alpha << std::endl; + + return QColor(red, green, blue, alpha); +} + +///////////////////////////////////////////////// +void DustManager::ConfigureEmitter(std::string _emitter_name) +{ + gzmsg << "Selected Dust Model: " << _emitter_name << std::endl; + this->dataPtr->emitterCurrentPtr = this->dataPtr->emitterMap[_emitter_name]; + + if (this->dataPtr->emitterCurrentPtr) + { + // Getting all the paremeters from particle emitter and setting + // them to the local variables + this->lifetime = this->dataPtr->emitterCurrentPtr->Lifetime(); + this->rate = this->dataPtr->emitterCurrentPtr->Rate(); + this->scaleRate = this->dataPtr->emitterCurrentPtr->ScaleRate(); + this->size = this->dataPtr->emitterCurrentPtr->EmitterSize(); + this->pose = this->dataPtr->emitterCurrentPtr->LocalPose(); + this->material = this->dataPtr->emitterCurrentPtr->Material(); + std::string imagePath = material->Texture(); + + if (!this->image.Load(imagePath)) + { + gzmsg << "Image Loaded Successfully" << std::endl; + } + gz::math::Color avgColor = this->image.AvgColor(); + + this->emitting = this->dataPtr->emitterCurrentPtr->Emitting(); + this->velocity = this->dataPtr->emitterCurrentPtr->MaxVelocity(); + this->density = GetDensity(lifetime, rate, scaleRate, size); + this->sizeX = size.X(); + this->sizeY = size.Y(); + this->sizeZ = size.Z(); + this->poseX = pose.Pos().X(); + this->poseY = pose.Pos().Y(); + this->poseZ = pose.Pos().Z(); + this->angle = pose.Rot().Euler().Z(); + this->dustColor = GzColorToQColor(avgColor); + + emittingChanged(); + velocityChanged(); + densityChanged(); + sizeXChanged(); + sizeYChanged(); + sizeZChanged(); + poseXChanged(); + poseYChanged(); + poseZChanged(); + angleChanged(); + dustColorChanged(); + } +} + +///////////////////////////////////////////////// +void DustManager::ToggleDust(bool _checked) +{ + if (this->dataPtr->emitterCurrentPtr) + { + if (_checked) + { + this->dataPtr->emitterCurrentPtr->SetEmitting(true); + gzmsg << "Dust On" << std::endl; + } + else + { + this->dataPtr->emitterCurrentPtr->SetEmitting(false); + gzmsg << "Dust Off" << std::endl; + } + } + else + { + gzmsg << "Dust Emitter Not Set" << std::endl; + } +} + +//////////////////////////////////////////////// +void DustManager::ToggleWind(bool _checked) +{ + gz::msgs::Boolean enable_wind; + enable_wind.set_data(_checked); + this->dataPtr->windMsg->set_enable_wind(_checked); + + if (_checked) + { + gzmsg << "Wind On" << std::endl; + } + else + { + gzmsg << "Wind Off" << std::endl; + } + + SetWindVelocity(this->velocity, this->angle); +} + +// //////////////////////////////////////////////// +void DustManager::OnDensityChanged(double _density) +{ + // Calcuating Lifetime and Rate using Density on set size + double volume = this->sizeX * this->sizeY * this->sizeZ; + double totalParticles = volume * _density; + this->rate = std::sqrt(totalParticles); + this->lifetime = totalParticles / (this->rate * this->scaleRate); + this->dataPtr->emitterCurrentPtr->SetLifetime(this->lifetime); + this->dataPtr->emitterCurrentPtr->SetRate(this->rate); +} + +//////////////////////////////////////////////// +void DustManager::SetWindVelocity(double _velocity, double _angle) +{ + // Setting Velocity of Wind + gz::msgs::Vector3d *wind_linear_velocity = new gz::msgs::Vector3d(); + wind_linear_velocity->set_x(_velocity * cos(_angle)); + wind_linear_velocity->set_y(_velocity * sin(_angle)); + gzmsg << "Wind Velocity: " << _velocity << ", " << _angle << std::endl; + this->dataPtr->windMsg->set_allocated_linear_velocity(wind_linear_velocity); + gzmsg << "Publishing Wind" << std::endl; + this->dataPtr->windPublisher.Publish(*(this->dataPtr->windMsg)); +} + +// //////////////////////////////////////////////// +void DustManager::OnVelocityChanged(double _velocity) +{ + // Setting Velocity of Particle Emitter + this->velocity = _velocity; + this->dataPtr->emitterCurrentPtr->SetVelocityRange(_velocity - 0.3, _velocity + 0.3); + + SetWindVelocity(this->velocity, this->angle); +} + +// //////////////////////////////////////////////// +void DustManager::OnPoseChanged(double _x, double _y, double _z, double _angle) +{ + this->angle = _angle; + + SetWindVelocity(this->velocity, this->angle); + + this->dataPtr->emitterCurrentPtr->SetLocalPose({_x, _y, _z, 0, 0, _angle}); + gzmsg << "Pose Changed: " << _x << ", " << _y << ", " << _z << std::endl; +} + +// //////////////////////////////////////////////// +void DustManager::OnSizeChanged(double _x, double _y, double _z) +{ + this->dataPtr->emitterCurrentPtr->SetEmitterSize({_x, _y, _z}); + gzmsg << "Size Changed: " << _x << ", " << _y << ", " << _z << std::endl; + + // Calculating new Rate and Scale rate to maintain the density + double originalVolume = sizeX * sizeY * sizeZ; + double newVolume = _x * _y * _z; + double totalParticles = density * newVolume; + double newRate = this->rate * (newVolume / originalVolume); + double newScaleRate = totalParticles / (newRate * this->lifetime); + + this->dataPtr->emitterCurrentPtr->SetRate(newRate); + this->dataPtr->emitterCurrentPtr->SetScaleRate(newScaleRate); +} + +// //////////////////////////////////////////////// +void DustManager::OnColorChanged(const QColor _dustColor) +{ + // if (!this->image.Data().empty()) + // { + // gzmsg << " IMage date not found" << std::endl; + // return; + // } + + gzmsg << "Changing Color" << std::endl; + gzmsg << "Got New Colors: " << _dustColor.red() << ", " << _dustColor.green() << ", " << _dustColor.blue() << std::endl; + + this->dustColor = _dustColor; + dustColorChanged(); + + auto data = this->image.Data(); + + for (size_t y = 0; y < this->image.Height(); ++y) + { + for (size_t x = 0; x < this->image.Width(); ++x) + { + // Get the index of the current pixel in the data array + size_t index = (y * this->image.Width() + x) * 4; + + // Set the red, green, and blue values + data[index + 0] = _dustColor.red(); // Red + data[index + 1] = _dustColor.green(); // Green + data[index + 2] = _dustColor.blue(); // Blue + + this->image.SetFromData(data.data(), this->image.Width(), this->image.Height(), this->image.PixelFormat()); + + this->changeTexture = true; + } + } + +} + +} + +// Register this plugin +GZ_ADD_PLUGIN(dust_manager::DustManager, + gz::gui::Plugin) diff --git a/custom_gz_plugins/src/DustManager/DustManager.hh b/custom_gz_plugins/src/DustManager/DustManager.hh new file mode 100644 index 00000000..10a76154 --- /dev/null +++ b/custom_gz_plugins/src/DustManager/DustManager.hh @@ -0,0 +1,200 @@ +/* + * Copyright (C) 2024 Naman Malik namanmalik0210@gmail.com + * + * 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 GZ_GUI_PLUGINS_DustManager_HH_ +#define GZ_GUI_PLUGINS_DustManager_HH_ + +#include + +#include "gz/gui/Plugin.hh" +#include +#include + +#include +#include +#include +#include +#include + + +using namespace gz; +using namespace sim; +using namespace systems; + +namespace dust_manager +{ +class DustManagerPrivate; + +class DustManager : public gz::sim::GuiSystem +{ + Q_OBJECT + + /// \brief Constructor + public: DustManager(); + + /// \brief Destructor + public: ~DustManager(); + + /// \brief Q_Property function for changing the values in the GUI + Q_PROPERTY(QStringList dustModelList READ dustModelList WRITE SetdustModelList NOTIFY dustModelListChanged) + Q_PROPERTY(bool emitting MEMBER emitting NOTIFY emittingChanged) + Q_PROPERTY(double velocity MEMBER velocity NOTIFY velocityChanged) + Q_PROPERTY(double density MEMBER density NOTIFY densityChanged) + Q_PROPERTY(double sizeX MEMBER sizeX NOTIFY sizeXChanged) + Q_PROPERTY(double sizeY MEMBER sizeY NOTIFY sizeYChanged) + Q_PROPERTY(double sizeZ MEMBER sizeZ NOTIFY sizeZChanged) + Q_PROPERTY(double poseX MEMBER poseX NOTIFY poseXChanged) + Q_PROPERTY(double poseY MEMBER poseY NOTIFY poseYChanged) + Q_PROPERTY(double poseZ MEMBER poseZ NOTIFY poseZChanged) + Q_PROPERTY(double angle MEMBER angle NOTIFY angleChanged) + Q_PROPERTY(QColor dustColor MEMBER dustColor NOTIFY dustColorChanged) + + /// \brief List of Dust Model Parameters + public: bool emitting = false; + public: gz::math::Vector3d size; + public: gz::math::Pose3d pose; + public: double lifetime; + public: double rate; + public: double scaleRate; + public: double density; + public: double velocity; + public: double poseX; + public: double poseY; + public: double poseZ; + public: double sizeX; + public: double sizeY; + public: double sizeZ; + public: double angle; + public: QColor dustColor = QColor(255, 255, 255); + public: QColor dustColorBorder = QColor(0, 0, 0); + gz::common::Image image; + rendering::MaterialPtr material; + public: bool changeTexture = false; + + /// \brief Notify that Model list has changed + signals: void dustModelListChanged(); + signals: void velocityChanged(); + signals: void densityChanged(); + signals: void emittingChanged(); + signals: void sizeXChanged(); + signals: void sizeYChanged(); + signals: void sizeZChanged(); + signals: void poseXChanged(); + signals: void poseYChanged(); + signals: void poseZChanged(); + signals: void angleChanged(); + signals: void dustColorChanged(); + + /// \return List of models + public: Q_INVOKABLE QStringList dustModelList() const; + + /// \brief Set the topic list from a string + /// \param[in] dustModelList List of topics. + public: Q_INVOKABLE void SetdustModelList( + const QStringList &_dustModelList); + + /// \brief Load parameters + /// \param[in] _pluginElem plugin element + public: Q_INVOKABLE void LoadConfig(const tinyxml2::XMLElement *_pluginElem); + + /// \brief Callback when dust emitter topic is changed + /// \param[in] _dustEmitterTopic + public: Q_INVOKABLE void OnDustEmitterModel(const QString &_dustEmitterModel); + + /// \brief Callback for density changd + /// \param[in] _density + public: Q_INVOKABLE void OnDensityChanged(double _density); + + /// \brief Callback for angle changed + /// \param[in] _angle + public: Q_INVOKABLE void OnAngleChanged(double _angle); + + /// \brief Callback for velocity changed + /// \param[in] _velocity + public: Q_INVOKABLE void OnVelocityChanged(double _velocity); + + /// \brief Callback for pose changed + /// \param[in] _x + /// \param[in] _y + /// \param[in] _z + /// \param[in] _angle + public: Q_INVOKABLE void OnPoseChanged(double _x, double _y, double _z, double _angle); + + /// \brief Callback for size changed + /// \param[in] _x + /// \param[in] _y + /// \param[in] _z + public: Q_INVOKABLE void OnSizeChanged(double _x, double _y, double _z); + + /// \brief Callback for color changed + /// \param[in] _dustColor + public: Q_INVOKABLE void OnColorChanged(const QColor _dustColor); + + /// \brief Callback when dust on/off checkbox state is changed + /// \param[in] _checked indicates show or hide contacts + public: Q_INVOKABLE void ToggleDust(bool _checked); + + /// \brief Callback when wind on/off checkbox state is changed + /// \param[in] _checked indicates show or hide contacts + public: Q_INVOKABLE void ToggleWind(bool _checked); + + /// \brief Update function + /// \param[in] _info + /// \param[in] _ecm + public: void Update(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + public: Q_INVOKABLE void SetupWindTopic(); + + /// @brief Find all the visuals in the scene to get the ParticleEmitter pointer + /// @param _ecm + public: void FindEntityVisual( + gz::sim::EntityComponentManager &_ecm); + + /// @brief Configure the emitter on selected model + /// @param _emitter_name + public: void ConfigureEmitter(std::string _emitter_name); + + /// @brief Get the density of the particle + /// @param _lifetime + /// @param _rate + /// @param _scalRate + /// @param _size + public: double GetDensity(double _lifetime, double _rate, double _scalRate, gz::math::Vector3d _size); + + /// @brief Change the base color of the image + /// @param image + /// @param color + public: gz::common::Image ChangeImageBaseColor(gz::common::Image image, const QColor color); + + /// @brief Set the wind velocity + /// @param _velocity + /// @param _windAngle + public: void SetWindVelocity(double _velocity, double _windAngle); + + /// \brief Qt event filter + /// \param _obj + /// \param _event + public: bool eventFilter(QObject *_obj, QEvent *_event); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; +}; +} + +#endif // GZ_GUI_PLUGINS_DustManager_HH_ \ No newline at end of file diff --git a/custom_gz_plugins/src/DustManager/DustManager.qml b/custom_gz_plugins/src/DustManager/DustManager.qml new file mode 100644 index 00000000..61ed3dda --- /dev/null +++ b/custom_gz_plugins/src/DustManager/DustManager.qml @@ -0,0 +1,259 @@ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import gz.gui 1.0 +import "qrc:/qml" + +ColumnLayout { + spacing: 20 + Layout.minimumWidth: 590 + Layout.minimumHeight: 590 + anchors.fill: parent + anchors.margins: 20 + + RowLayout { + spacing: 20 + Layout.fillWidth: true + + Switch { + Layout.alignment: Qt.AlignVCenter + id: dustVisual + Layout.columnSpan: 5 + Layout.fillWidth: true + text: qsTr("Dust") + checked: DustManager.emitting + onToggled: { + DustManager.ToggleDust(checked) + } + font.pixelSize: 16 + } + } + + RowLayout { + spacing: 20 + Layout.fillWidth: true + + Switch { + Layout.alignment: Qt.AlignVCenter + id: windVisual + Layout.columnSpan: 5 + Layout.fillWidth: true + text: qsTr("Wind") + checked: false + onToggled: { + DustManager.ToggleWind(checked) + } + font.pixelSize: 16 + } + + RowLayout { + spacing: 10 + Layout.fillWidth: true + + Label { + Layout.columnSpan: 1 + text: "Color" + font.pixelSize: 14 + } + + Button { + Layout.columnSpan: 1 + id: minColorButton + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + ToolTip.text: qsTr("Color for minimum value") + onClicked: dustColorDialog.open() + background: Rectangle { + implicitWidth: 40 + implicitHeight: 40 + radius: 5 + border.color: DustManager.dustColorBorder + border.width: 2 + color: DustManager.dustColor + } + } + + ColorDialog { + id: dustColorDialog + title: "Choose a color for the minimum value" + visible: false + onAccepted: { + DustManager.OnColorChanged(dustColorDialog.color) + dustColorDialog.close() + } + onRejected: { + dustColorDialog.close() + } + } + } + } + + GridLayout { + columns: 3 + columnSpacing: 20 + rowSpacing: 15 + Layout.fillWidth: true + + Label { + Layout.columnSpan: 1 + text: "Emitter Model" + font.pixelSize: 14 + } + + ComboBox { + Layout.columnSpan: 2 + id: emitterCombo + Layout.fillWidth: true + model: DustManager.dustModelList + currentIndex: 0 + onCurrentIndexChanged: { + if (currentIndex < 0) + return; + DustManager.OnDustEmitterModel(textAt(currentIndex)); + } + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + ToolTip.text: qsTr("Gazebo Transport topics publishing Dust messages") + } + } + + ColumnLayout { + spacing: 15 + + GridLayout { + columns: 4 + columnSpacing: 10 + rowSpacing: 10 + + Label { + Layout.alignment: Qt.AlignVCenter + text: "Density" + font.pixelSize: 14 + } + + GzSpinBox { + id: dustDensity + Layout.fillWidth: true + value: DustManager.density + minimumValue: 1 + maximumValue: 1000 + decimals: 0 + onEditingFinished: { + DustManager.OnDensityChanged(dustDensity.value) + } + } + + Label { + Layout.alignment: Qt.AlignVCenter + text: "Angle" + font.pixelSize: 14 + } + + GzSpinBox { + id: angleSpin + Layout.fillWidth: true + value: DustManager.angle + minimumValue: -3 + maximumValue: 3 + decimals: 0 + onEditingFinished: { + DustManager.OnPoseChanged(poseXSpin.value, poseYSpin.value, poseZSpin.value, angleSpin.value) + } + } + + Label { + Layout.alignment: Qt.AlignVCenter + text: "Velocity" + font.pixelSize: 14 + } + + GzSpinBox { + id: velocitySpin + Layout.fillWidth: true + value: DustManager.velocity + minimumValue: 1 + maximumValue: 1000 + decimals: 0 + onEditingFinished: { + DustManager.OnVelocityChanged(velocitySpin.value) + } + } + } + + ColumnLayout { + spacing: 10 + + Label { + text: "Pose" + font.pixelSize: 16 + font.bold: true + } + + GridLayout { + columns: 6 + columnSpacing: 10 + rowSpacing: 5 + + Label { text: "X"; font.pixelSize: 14 } + GzSpinBox { id: poseXSpin; value: DustManager.poseX; minimumValue: -1000; maximumValue: 1000; decimals: 0 + onEditingFinished: { + DustManager.OnPoseChanged(poseXSpin.value, poseYSpin.value, poseZSpin.value, angleSpin.value) + } + } + + Label { text: "Y"; font.pixelSize: 14 } + GzSpinBox { id: poseYSpin; value: DustManager.poseY; minimumValue: -1000; maximumValue: 1000; decimals: 0 + onEditingFinished: { + DustManager.OnPoseChanged(poseXSpin.value, poseYSpin.value, poseZSpin.value, angleSpin.value) + } + } + + Label { text: "Z"; font.pixelSize: 14 } + GzSpinBox { id: poseZSpin; value: DustManager.poseZ; minimumValue: -1000; maximumValue: 1000; decimals: 0 + onEditingFinished: { + DustManager.OnPoseChanged(poseXSpin.value, poseYSpin.value, poseZSpin.value, angleSpin.value) + } + } + } + } + + ColumnLayout { + spacing: 10 + + Label { + text: "Volume" + font.pixelSize: 16 + font.bold: true + } + + GridLayout { + columns: 6 + columnSpacing: 10 + rowSpacing: 5 + + Label { text: "L"; font.pixelSize: 14 } + GzSpinBox { id: volumeLSpin; value: DustManager.sizeX; minimumValue: 0; maximumValue: 100; decimals: 0 + onEditingFinished: { + DustManager.OnSizeChanged(volumeLSpin.value, volumeHSpin.value, volumeBSpin.value) + } + } + + Label { text: "B"; font.pixelSize: 14 } + GzSpinBox { id: volumeBSpin; value: DustManager.sizeX; minimumValue: 0; maximumValue: 100; decimals: 0 + onEditingFinished: { + DustManager.OnSizeChanged(volumeLSpin.value, volumeHSpin.value, volumeBSpin.value) + } + } + + Label { text: "H"; font.pixelSize: 14 } + GzSpinBox { id: volumeHSpin; value: DustManager.sizeZ; minimumValue: 0; maximumValue: 100; decimals: 0 + onEditingFinished: { + DustManager.OnSizeChanged(volumeLSpin.value, volumeHSpin.value, volumeBSpin.value) + } + } + } + } + } +} \ No newline at end of file diff --git a/custom_gz_plugins/src/DustManager/DustManager.qrc b/custom_gz_plugins/src/DustManager/DustManager.qrc new file mode 100644 index 00000000..6fb4b880 --- /dev/null +++ b/custom_gz_plugins/src/DustManager/DustManager.qrc @@ -0,0 +1,5 @@ + + + DustManager.qml + + diff --git a/custom_gz_plugins/src/DustManager/README.md b/custom_gz_plugins/src/DustManager/README.md new file mode 100644 index 00000000..3bd7780e --- /dev/null +++ b/custom_gz_plugins/src/DustManager/README.md @@ -0,0 +1,39 @@ +# Dust Manager Plugin + +## Introduction +The Dust Manager plugin simulates realistic dust dynamics in Martian or space environments by controlling particle emitters. Dust is a significant factor in extraterrestrial conditions, particularly on Mars, where it can affect visibility, equipment performance, and overall mission success. This plugin allows for the accurate simulation of dust clouds, aiding in the development and testing of vehicles and systems designed for planetary exploration. By mimicking how dust interacts with moving objects, the plugin helps create more realistic and challenging simulation scenarios for space missions. +> The Dust Manager Plugin also provides feature to control the wind depending upon the paramters of the dust storms added in the simulation. + +## Usage and Features +![Dust Manager Featurs](assets/dust_test.gif) + +> As seen the in the image above, the dust is visable in the RGB camera and adds noise to the depth camera as well. Dust noise particles can also be seeen in the pointcloud data from the 3D lidar + +![Dust Manager Featurs](assets/dust_noise.gif) + +### Color Change +You can change the color of the dust using the Dust Manager plugin GUI. +The color can be set the desired color or can be picked up from the screen. +![Dust Color Change](assets/dust_color.gif) + +### Dust Models +In order to spawn the dust model into the simulation, open the resource spawner plugin and search for Dust Storm models. There are 2 dust storm models available: +- Light Dust Storm: For light dust storm. +- Dust Storm: For heavy dust. + +![Dust Models](assets/models.png) + +### Dust Manager GUI +The dust models are highly configurable using the dust manager GUI. In order to use the GUI, spawn the GUI from the top-right corner menu and spawn a dust storm model. The GUI will autmotically configure itself to the latest added dust storm model. +Various parameters of the dust storm can be configured such as: +- Toggle Dust: On/Off +- Toggle Wind: On/Off +- Select Dust Storm Model +- Control Density +- Control Volocity of dust particles +- Angle and Position of Dust Emitter +- Size of Dust Emitter + +![Dust Manager GUI](assets/gui.png) + + diff --git a/custom_gz_plugins/src/DustManager/assets/dust_color.gif b/custom_gz_plugins/src/DustManager/assets/dust_color.gif new file mode 100644 index 00000000..77619849 Binary files /dev/null and b/custom_gz_plugins/src/DustManager/assets/dust_color.gif differ diff --git a/custom_gz_plugins/src/DustManager/assets/dust_noise.gif b/custom_gz_plugins/src/DustManager/assets/dust_noise.gif new file mode 100644 index 00000000..04aa85ae Binary files /dev/null and b/custom_gz_plugins/src/DustManager/assets/dust_noise.gif differ diff --git a/custom_gz_plugins/src/DustManager/assets/dust_test.gif b/custom_gz_plugins/src/DustManager/assets/dust_test.gif new file mode 100644 index 00000000..4c763a93 Binary files /dev/null and b/custom_gz_plugins/src/DustManager/assets/dust_test.gif differ diff --git a/custom_gz_plugins/src/DustManager/assets/gui.png b/custom_gz_plugins/src/DustManager/assets/gui.png new file mode 100644 index 00000000..0092e479 Binary files /dev/null and b/custom_gz_plugins/src/DustManager/assets/gui.png differ diff --git a/custom_gz_plugins/src/DustManager/assets/models.png b/custom_gz_plugins/src/DustManager/assets/models.png new file mode 100644 index 00000000..2d990b23 Binary files /dev/null and b/custom_gz_plugins/src/DustManager/assets/models.png differ diff --git a/custom_gz_plugins/src/VehicleDust/CMakeLists.txt b/custom_gz_plugins/src/VehicleDust/CMakeLists.txt new file mode 100644 index 00000000..78aca4e7 --- /dev/null +++ b/custom_gz_plugins/src/VehicleDust/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(gz-cmake3 REQUIRED) + +project(CasDust) + +find_package(gz-plugin2 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + +find_package(gz-sim8 REQUIRED) +find_package(GTest REQUIRED) + +add_library(VehicleDust SHARED VehicleDust.cc ) +set_property(TARGET VehicleDust PROPERTY CXX_STANDARD 17) +target_link_libraries(VehicleDust + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim8::gz-sim8) + +install(TARGETS VehicleDust + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) \ No newline at end of file diff --git a/custom_gz_plugins/src/VehicleDust/README.md b/custom_gz_plugins/src/VehicleDust/README.md new file mode 100644 index 00000000..f466aac0 --- /dev/null +++ b/custom_gz_plugins/src/VehicleDust/README.md @@ -0,0 +1,26 @@ +# Vehicle Dust Plugin +The Dust Emission Plugin not only simulates dust clouds generated by heavy vehicles on planetary surfaces like Mars or the Moon, but it also impacts the performance of various sensors such as depth cameras, RGB cameras, and 3D LiDAR. + +- Depth Cameras: Dust particles may scatter infrared light, leading to noise in depth data or complete failure to capture the scene accurately. + +- RGB Cameras: Dust clouds can reduce visibility for RGB cameras, as they create a haze that obstructs clear imagery. This reduction in clarity can affect visual navigation, object detection, and color-based tracking. + +- 3D LiDAR: Dust interferes with LiDAR systems by scattering laser beams, causing reflections that may distort the returned signal. This can result in inaccurate distance measurements and false readings of the environment, compromising obstacle detection and mapping. + +By simulating dust and its interactions with sensors, the plugin provides a more challenging and realistic testing environment, helping to ensure that sensor-based systems function effectively in harsh planetary conditions. + + +## Usage + +```xml + + /cmd_vel + buggy_model + + +``` +- In order to load the pugin, add the above plugin tag into the world sdf and configure the following tags: + - topic: The command interface topic used to drive the robot + - emitter_model: Name of the robot in the simultion \ No newline at end of file diff --git a/custom_gz_plugins/src/VehicleDust/VehicleDust.cc b/custom_gz_plugins/src/VehicleDust/VehicleDust.cc new file mode 100644 index 00000000..5ef29d3b --- /dev/null +++ b/custom_gz_plugins/src/VehicleDust/VehicleDust.cc @@ -0,0 +1,108 @@ +/* +The `VehicleDustPrivate` is the main script which create a ignition gazebo node in oder to handle communication between the camera topics. +A subsciber is created which subscribes to the `/model/car/cmd_vel` topic build published by the camera plugin in the gazebo world. +The image recieved is first converted into opencv format using the GzCVBridge script on top of which a custom post processing +algorithm is applied using openCV in order to add the rain effect noise on the camera the frame. The rain augmented frame in them +published on a seperate topic called `/model/car/link/dust_link/particle_emitter/emitter/cmd`. +*/ + +#include +#include +#include +#include +#include +#include +#include "VehicleDust.hh" +#include + + +namespace vehicle_dust +{ + class VehicleDustPrivate + { + public: gz::transport::Node velDustNode; + public: std::string vel_topic; + public: std::string dust_topic; + public: gz::transport::Node::Publisher dustPub; + public: gz::msgs::Vector3d linear; + public: double scale_rate = 0, rate = 0, linear_vel = 0; + + public: void publish_dust(double rate_calculated, double scale_rate_calculated); + public: void cmd_vel_cb(const gz::msgs::Twist &_msg); + }; + + void VehicleDustPrivate::publish_dust(double rate_calculated, double scale_rate_calculated) + { + + + gz::msgs::ParticleEmitter *msg = new gz::msgs::ParticleEmitter(); + gz::msgs::Float rate, scale_rate; + + rate.set_data(rate_calculated); + scale_rate.set_data(scale_rate_calculated); + + msg->set_allocated_rate(&rate); + msg->set_allocated_scale_rate(&scale_rate); + + this->dustPub.Publish(*msg); + + } + + /* + Callback function for the image frame recieved by the camera plugin. The function initially sets the frame width and frame height + for further processing. The function recieves the final rain augmented frame by calling the `add_rain_effect` & `add_text` functions + and then finally outputs the frame using `publish_dust` function. + */ + void VehicleDustPrivate::cmd_vel_cb(const gz::msgs::Twist &_msg) + { + this->linear = _msg.linear(); + this->linear_vel = abs(this->linear.x()); + + if(this->linear_vel > MAX_VEL) + { + this->linear_vel = MAX_VEL; + } + + this->scale_rate = this->linear_vel * SCALE_CONST; + this->rate = this->linear_vel * RATE_CONST; + + // std::cout<< "Rate: "<< this->rate << " | ScaleRate: " << this->scale_rate << std::endl; + + this->publish_dust(this->rate, this->scale_rate); + } + + VehicleDust::VehicleDust() + : dataPtr(std::make_unique()) + {} + + VehicleDust::~VehicleDust() + {} + + void VehicleDust::Configure(const gz::sim::Entity &, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &, + gz::sim::EventManager &) + { + std::string velTopic = "/model/car/cmd_vel"; + std::string emitterModel = "car"; + + if(_sdf->HasElement("topic")) + velTopic = _sdf->Get("topic"); + if(_sdf->HasElement("emitter_model")) + emitterModel = _sdf->Get("emitter_model"); + + std::string emitterTopic = "/model/" + emitterModel + "/link/dust_link/particle_emitter/emitter/cmd"; + std::string velTopicProcessed = gz::transport::TopicUtils::AsValidTopic(velTopic); + std::string emitterTopicProcessed = gz::transport::TopicUtils::AsValidTopic(emitterTopic); + + this->dataPtr->velDustNode.Subscribe(velTopicProcessed, &VehicleDustPrivate::cmd_vel_cb, this->dataPtr.get()); + + this->dataPtr->dustPub = this->dataPtr->velDustNode.Advertise(emitterTopic); + } +} + + GZ_ADD_PLUGIN( + vehicle_dust::VehicleDust, + gz::sim::System, + vehicle_dust::VehicleDust::ISystemConfigure + ) \ No newline at end of file diff --git a/custom_gz_plugins/src/VehicleDust/VehicleDust.hh b/custom_gz_plugins/src/VehicleDust/VehicleDust.hh new file mode 100644 index 00000000..3c2d0b6d --- /dev/null +++ b/custom_gz_plugins/src/VehicleDust/VehicleDust.hh @@ -0,0 +1,34 @@ +#include +#include +#include +#include + +#define SCALE_CONST 1.6 +#define RATE_CONST 2.5 + +// used if rain is set as high or low +// random int between these can be used if rain is medium +#define MAX_VEL 5.0 + +// rain drop maturity means that as the drop falls it gathers more water and falls faster and becomes more blurry +namespace vehicle_dust +{ + class VehicleDustPrivate; + + class VehicleDust: + public gz::sim::System, + public gz::sim::ISystemConfigure +{ + public: VehicleDust(); + + public: ~VehicleDust() override; + + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + private: std::unique_ptr dataPtr; + +}; +} \ No newline at end of file diff --git a/custom_gz_plugins/worlds/drone_dust_demo.sdf b/custom_gz_plugins/worlds/drone_dust_demo.sdf new file mode 100644 index 00000000..38bb313e --- /dev/null +++ b/custom_gz_plugins/worlds/drone_dust_demo.sdf @@ -0,0 +1,218 @@ + + + + + 0.05 + 1 + 1000 + + + + + + + + + + ogre2 + + + + + + + + + + + + 3D View + false + docked + + ogre2 + 1.0 1.0 1.0 + 0.8 0.8 0.8 + 0 0 5 0 0 0 + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + /world/world_demo/control + /world/world_demo/stats + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + /world/world_demo/stats + + + + + + + + + false + 5 + 5 + floating + false + + + + + Transform control + + + + + false + 250 + 50 + floating + false + #03a9f4 + + + + 0 50 50 0 0 0 + orbit + perspective + + + + 0 0 -3.7 + 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 + + + + 0.400000006 0.400000006 0.400000006 1 + 1 0.412 0.05 1 + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 200 3500 + 0 0 1 + + + + + + + + + + + + + + 200 3500 + 0 0 1 + + + + 0.4 0.2 0.05 1 + 0.4 0.2 0.05 1 + 0.4 0.2 0.05 1 + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 0 0 + false + + + + + + ingenuity + model://ingenuity + 12.92 -20.10 0.27 0 3.14 0 + + + + 5 + 0.27 + 12.0 + ground_plane + ingenuity + + + + diff --git a/custom_gz_plugins/worlds/vehicle_dust_demo.sdf b/custom_gz_plugins/worlds/vehicle_dust_demo.sdf new file mode 100644 index 00000000..0978858f --- /dev/null +++ b/custom_gz_plugins/worlds/vehicle_dust_demo.sdf @@ -0,0 +1,168 @@ + + + + + 0.05 + 1 + 1000 + + + + + + + + + + ogre2 + + + + /cmd_vel + buggy_model + + + + + + + + + + 3D View + false + docked + + ogre2 + 1.0 1.0 1.0 + 0.8 0.8 0.8 + 0 0 5 0 0 0 + + + + World control + false + false + 72 + 121 + 1 + floating + + + + + + true + true + true + /world/world_demo/control + /world/world_demo/stats + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + /world/world_demo/stats + + + + + + + + + false + 5 + 5 + floating + false + + + + + docked + + + + + Transform control + + + + + false + 250 + 50 + floating + false + #03a9f4 + + + + 0 50 50 0 0 0 + orbit + perspective + + + + 0 0 -3.7 + 5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05 + + + + 0.400000006 0.400000006 0.400000006 1 + 1 0.412 0.05 1 + true + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + Gale_Crater_Patch1 + model://martian_terrains/gale_crater_patch1 + 0 0 0 0 0 0 + + + + buggy_model + model://buggy_model + 0 0 6 1.56 0 0 + + + + diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py index 91e3c99f..2e1f2a6a 100644 --- a/mars_rover/launch/mars_rover.launch.py +++ b/mars_rover/launch/mars_rover.launch.py @@ -23,12 +23,12 @@ def generate_launch_description(): mars_rover_demos_path = get_package_share_directory('mars_rover') mars_rover_models_path = get_package_share_directory('simulation') - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': - ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': + ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''), environ.get('LD_LIBRARY_PATH', default='')]), - 'IGN_GAZEBO_RESOURCE_PATH': - ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), mars_rover_demos_path])} - + 'GZ_SIM_RESOURCE_PATH': + ':'.join([mars_rover_demos_path, mars_rover_models_path + '/models'])} + urdf_model_path = os.path.join(mars_rover_models_path, 'models', 'curiosity_path', 'urdf', 'curiosity_mars_rover.xacro') mars_world_model = os.path.join(mars_rover_demos_path, 'worlds', 'mars_curiosity.world') @@ -67,7 +67,7 @@ def generate_launch_description(): ) start_world = ExecuteProcess( - cmd=['ign gazebo', mars_world_model, '-r'], + cmd=['gz sim', mars_world_model, '-r'], output='screen', additional_env=env, shell=True @@ -97,7 +97,7 @@ def generate_launch_description(): output='screen') spawn = Node( - package='ros_ign_gazebo', executable='create', + package='ros_gz_sim', executable='create', arguments=[ '-name', 'curiosity_mars_rover', '-topic', robot_description, @@ -110,7 +110,7 @@ def generate_launch_description(): ## Control Components - component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}' + component_state_msg = '{name: "GazeboSimSystem", target_state: {id: 3, label: ""}}' ## a hack to resolve current bug set_hardware_interface_active = ExecuteProcess( diff --git a/mars_rover/nodes/move_arm b/mars_rover/nodes/move_arm old mode 100644 new mode 100755 diff --git a/mars_rover/nodes/move_mast b/mars_rover/nodes/move_mast old mode 100644 new mode 100755 diff --git a/mars_rover/nodes/move_wheel b/mars_rover/nodes/move_wheel old mode 100644 new mode 100755 diff --git a/mars_rover/nodes/run_demo b/mars_rover/nodes/run_demo old mode 100644 new mode 100755 diff --git a/mars_rover/worlds/mars_curiosity.world b/mars_rover/worlds/mars_curiosity.world index a5c036fa..1e3f63eb 100755 --- a/mars_rover/worlds/mars_curiosity.world +++ b/mars_rover/worlds/mars_curiosity.world @@ -1,58 +1,246 @@ - + + - + + 0.001 + 1.0 + + + + + + + + + ogre2 + - - - 3D View - false - docked - + + + + 3D View + false + docked + ogre2 scene - 0.9 0.753 0.66 - -5.0 0.0 -6.0 0.0 0.0 0.0 + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + - - - floating - + + + false + 5 + 5 + floating + false + - - - - - - - - ogre2 - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + false + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + - 0 0 -3.711 + 0 0 -3.711 model://curiosity_path diff --git a/mars_rover/worlds/mars_gale_crater_patch1.sdf b/mars_rover/worlds/mars_gale_crater_patch1.sdf new file mode 100644 index 00000000..f4aeba6b --- /dev/null +++ b/mars_rover/worlds/mars_gale_crater_patch1.sdf @@ -0,0 +1,250 @@ + + + + + 0.001 + 1.0 + + + + + + + + + ogre2 + + + + + + 3D View + false + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + false + docked + + + + + 0 0 -3.711 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + https://fuel.gazebosim.org/1.0/jasmeetsingh/models/Mars Gale Crater Patch 1 + + + + diff --git a/mars_rover/worlds/mars_gale_crater_patch2.sdf b/mars_rover/worlds/mars_gale_crater_patch2.sdf new file mode 100644 index 00000000..d0e2f11b --- /dev/null +++ b/mars_rover/worlds/mars_gale_crater_patch2.sdf @@ -0,0 +1,250 @@ + + + + + 0.001 + 1.0 + + + + + + + + + ogre2 + + + + + + 3D View + false + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + false + docked + + + + + 0 0 -3.711 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + https://fuel.gazebosim.org/1.0/jasmeetsingh/models/Mars Gale Crater Patch 2 + + + + diff --git a/mars_rover/worlds/moon_60s_30s.sdf b/mars_rover/worlds/moon_60s_30s.sdf new file mode 100644 index 00000000..dc10516c --- /dev/null +++ b/mars_rover/worlds/moon_60s_30s.sdf @@ -0,0 +1,250 @@ + + + + + 0.001 + 1.0 + + + + + + + + + ogre2 + + + + + + 3D View + false + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 1 + floating + + + + + + true + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + floating + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + false + docked + + + + + 0 0 -1.62 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + https://fuel.gazebosim.org/1.0/jasmeetsingh/models/Moon 60S 30S + + + +