diff --git a/examples/dave_demos/README.md b/examples/dave_demos/README.md index e913d4c3..8ebefe92 100644 --- a/examples/dave_demos/README.md +++ b/examples/dave_demos/README.md @@ -1,6 +1,6 @@ -## Examples +# Examples -### 1. Launching a Dave Object Model using Fuel URI +## Launching a Dave Object Model using Fuel URI To launch a Dave model directly from a Fuel URI, follow these steps: @@ -18,7 +18,7 @@ To launch a Dave model directly from a Fuel URI, follow these steps: This method simplifies the process by pulling the model directly from Fuel, ensuring you always have the latest version without needing to manage local files. -### 2. Launching a Dave Sensor Model using Downloaded Model Files +## Launching a Dave Sensor Model using Downloaded Model Files If you prefer to use model files downloaded from Fuel, proceed as follows: @@ -52,7 +52,39 @@ If you prefer to use model files downloaded from Fuel, proceed as follows: This approach gives you more control over the models you use, allowing for offline use and customization. It's especially useful when working in environments with limited internet connectivity or when specific model versions are required. -### 3. Launching a World File +## Launching a Dave Robot Model + +Before launching, ensure to build and source the workspace: + +```bash +colcon build && source install/setup.bash +``` + +1. Launching REXROV in an empty world: + +```bash +ros2 launch dave_demos dave_robot.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false +``` + +2. Launching REXROV in dave_ocean_waves.world: + +```bash +ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false +``` + +3. Launching Slocum Glider in an empty world: + +```bash +ros2 launch dave_demos dave_robot.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false +``` + +4. Launching Slocum Glider in dave_ocean_waves.world: + +```bash +ros2 launch dave_demos dave_robot.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false +``` + +## Launching a World File To launch a specific world file, you can specify the world name without the `.world` extension. Follow these steps: @@ -62,7 +94,7 @@ To launch a specific world file, you can specify the world name without the `.wo colcon build && source install/setup.bash ``` -1. Launch the world using the specified launch file +2. Launch the world using the specified launch file ```bash ros2 launch dave_demos dave_world.launch.py world_name:='dave_ocean_waves' @@ -70,6 +102,6 @@ ros2 launch dave_demos dave_world.launch.py world_name:='dave_ocean_waves' To check which worlds are available to launch, refer to `models/dave_worlds/worlds` directory. -The worlds files are linked to use models at https://app.gazebosim.org/ which means you need an internet connection to download the models and it takes some time to download at first launch. The files are saved in temporary directories and are reused in subsequent launches. +The world files are linked to use models at https://app.gazebosim.org/ which means you need an internet connection to download the models and it takes some time to download at first launch. The files are saved in temporary directories and are reused in subsequent launches. In this setup, you can dynamically specify different world files by changing the `world_name` argument in the launch command. diff --git a/examples/dave_demos/launch/dave_object.launch.py b/examples/dave_demos/launch/dave_object.launch.py index 46395a29..5568d595 100755 --- a/examples/dave_demos/launch/dave_object.launch.py +++ b/examples/dave_demos/launch/dave_object.launch.py @@ -1,44 +1,45 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import IfCondition from launch_ros.substitutions import FindPackageShare def launch_setup(context, *args, **kwargs): - paused = LaunchConfiguration("paused").perform(context) - gui = LaunchConfiguration("gui").perform(context) - use_sim_time = LaunchConfiguration("use_sim_time").perform(context) - headless = LaunchConfiguration("headless").perform(context) - verbose = LaunchConfiguration("verbose").perform(context) - namespace = LaunchConfiguration("namespace").perform(context) - world_name = LaunchConfiguration("world_name").perform(context) - x = LaunchConfiguration("x").perform(context) - y = LaunchConfiguration("y").perform(context) - z = LaunchConfiguration("z").perform(context) - roll = LaunchConfiguration("roll").perform(context) - pitch = LaunchConfiguration("pitch").perform(context) - yaw = LaunchConfiguration("yaw").perform(context) - use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context) + paused = LaunchConfiguration("paused") + gui = LaunchConfiguration("gui") + use_sim_time = LaunchConfiguration("use_sim_time") + debug = LaunchConfiguration("debug") + headless = LaunchConfiguration("headless") + verbose = LaunchConfiguration("verbose") + namespace = LaunchConfiguration("namespace") + world_name = LaunchConfiguration("world_name") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + use_ned_frame = LaunchConfiguration("use_ned_frame") - if world_name != "empty.sdf": + if world_name.perform(context) != "empty.sdf": + world_name = LaunchConfiguration("world_name").perform(context) world_filename = f"{world_name}.world" world_filepath = PathJoinSubstitution( [FindPackageShare("dave_worlds"), "worlds", world_filename] - ).perform(context) + ) gz_args = [world_filepath] else: gz_args = [world_name] - if headless == "true": - gz_args.append("-s") - if paused == "false": - gz_args.append("-r") - if verbose == "true": - gz_args.append("--verbose") - - gz_args_str = " ".join(gz_args) + if headless.perform(context) == "true": + gz_args.append(" -s") + if paused.perform(context) == "false": + gz_args.append(" -r") + if debug.perform(context) == "true": + gz_args.append(" -v ") + gz_args.append(verbose.perform(context)) gz_sim_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -53,7 +54,7 @@ def launch_setup(context, *args, **kwargs): ] ), launch_arguments=[ - ("gz_args", TextSubstitution(text=gz_args_str)), + ("gz_args", gz_args), ], condition=IfCondition(gui), ) @@ -105,6 +106,11 @@ def generate_launch_description(): default_value="true", description="Flag to indicate whether to use simulation time", ), + DeclareLaunchArgument( + "debug", + default_value="false", + description="Flag to enable the gazebo debug flag", + ), DeclareLaunchArgument( "headless", default_value="false", @@ -112,8 +118,8 @@ def generate_launch_description(): ), DeclareLaunchArgument( "verbose", - default_value="false", - description="Enable verbose mode for Gazebo simulation", + default_value="0", + description="Adjust level of console verbosity", ), DeclareLaunchArgument( "world_name", diff --git a/examples/dave_robot_launch/launch/robot_in_world.launch.py b/examples/dave_demos/launch/dave_robot.launch.py similarity index 100% rename from examples/dave_robot_launch/launch/robot_in_world.launch.py rename to examples/dave_demos/launch/dave_robot.launch.py diff --git a/examples/dave_demos/launch/dave_sensor.launch.py b/examples/dave_demos/launch/dave_sensor.launch.py index a436aae2..e41eaaa2 100644 --- a/examples/dave_demos/launch/dave_sensor.launch.py +++ b/examples/dave_demos/launch/dave_sensor.launch.py @@ -1,44 +1,45 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.conditions import IfCondition from launch_ros.substitutions import FindPackageShare def launch_setup(context, *args, **kwargs): - paused = LaunchConfiguration("paused").perform(context) - gui = LaunchConfiguration("gui").perform(context) - use_sim_time = LaunchConfiguration("use_sim_time").perform(context) - headless = LaunchConfiguration("headless").perform(context) - verbose = LaunchConfiguration("verbose").perform(context) - namespace = LaunchConfiguration("namespace").perform(context) - world_name = LaunchConfiguration("world_name").perform(context) - x = LaunchConfiguration("x").perform(context) - y = LaunchConfiguration("y").perform(context) - z = LaunchConfiguration("z").perform(context) - roll = LaunchConfiguration("roll").perform(context) - pitch = LaunchConfiguration("pitch").perform(context) - yaw = LaunchConfiguration("yaw").perform(context) - use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context) + paused = LaunchConfiguration("paused") + gui = LaunchConfiguration("gui") + use_sim_time = LaunchConfiguration("use_sim_time") + debug = LaunchConfiguration("debug") + headless = LaunchConfiguration("headless") + verbose = LaunchConfiguration("verbose") + namespace = LaunchConfiguration("namespace") + world_name = LaunchConfiguration("world_name") + x = LaunchConfiguration("x") + y = LaunchConfiguration("y") + z = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + use_ned_frame = LaunchConfiguration("use_ned_frame") - if world_name != "empty.sdf": + if world_name.perform(context) != "empty.sdf": + world_name = LaunchConfiguration("world_name").perform(context) world_filename = f"{world_name}.world" world_filepath = PathJoinSubstitution( [FindPackageShare("dave_worlds"), "worlds", world_filename] - ).perform(context) + ) gz_args = [world_filepath] else: gz_args = [world_name] - if headless == "true": - gz_args.append("-s") - if paused == "false": - gz_args.append("-r") - if verbose == "true": - gz_args.append("--verbose") - - gz_args_str = " ".join(gz_args) + if headless.perform(context) == "true": + gz_args.append(" -s") + if paused.perform(context) == "false": + gz_args.append(" -r") + if debug.perform(context) == "true": + gz_args.append(" -v ") + gz_args.append(verbose.perform(context)) gz_sim_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -53,7 +54,7 @@ def launch_setup(context, *args, **kwargs): ] ), launch_arguments=[ - ("gz_args", TextSubstitution(text=gz_args_str)), + ("gz_args", gz_args), ], condition=IfCondition(gui), ) @@ -105,6 +106,11 @@ def generate_launch_description(): default_value="true", description="Flag to indicate whether to use simulation time", ), + DeclareLaunchArgument( + "debug", + default_value="false", + description="Flag to enable the gazebo debug flag", + ), DeclareLaunchArgument( "headless", default_value="false", @@ -112,8 +118,8 @@ def generate_launch_description(): ), DeclareLaunchArgument( "verbose", - default_value="false", - description="Enable verbose mode for Gazebo simulation", + default_value="0", + description="Adjust level of console verbosity", ), DeclareLaunchArgument( "world_name", diff --git a/examples/dave_robot_launch/CMakeLists.txt b/examples/dave_robot_launch/CMakeLists.txt deleted file mode 100644 index bee48de8..00000000 --- a/examples/dave_robot_launch/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(dave_robot_launch) - -find_package(ament_cmake REQUIRED) - -install( - DIRECTORY launch - DESTINATION share/dave_robot_launch -) - -ament_package() diff --git a/examples/dave_robot_launch/README.md b/examples/dave_robot_launch/README.md deleted file mode 100644 index 1669f0ed..00000000 --- a/examples/dave_robot_launch/README.md +++ /dev/null @@ -1,31 +0,0 @@ -## Examples - -Before launching, ensure to build and source the workspace: - -```bash -colcon build && source install/setup.bash -``` - -### 1. Launching REXROV in an empty world - -```bash -ros2 launch dave_robot_launch robot_in_world.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false -``` - -### 2. Launching REXROV in dave_ocean_waves.world - -```bash -ros2 launch dave_robot_launch robot_in_world.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false -``` - -### 3. Launching Slocum Glider in an empty world - -```bash -ros2 launch dave_robot_launch robot_in_world.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false -``` - -### 4. Launching Slocum Glider in dave_ocean_waves.world - -```bash -ros2 launch dave_robot_launch robot_in_world.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false -``` diff --git a/examples/dave_robot_launch/package.xml b/examples/dave_robot_launch/package.xml deleted file mode 100644 index 0854972c..00000000 --- a/examples/dave_robot_launch/package.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - dave_robot_launch - 0.1.0 - A package that shows a demo to launch robot models. - Rakesh Vivekanandan - MIT - ament_cmake - - ament_cmake - - \ No newline at end of file diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt index 3fbb5cbc..93ff8c46 100644 --- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt +++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(dave_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(Protobuf REQUIRED) find_package(gz-msgs10 REQUIRED) +find_package(OpenCV REQUIRED) # Set version variables set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) @@ -21,15 +22,19 @@ set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) +include_directories(${OpenCV_INCLUDE_DIRS}) + message(STATUS "Compiling against Gazebo Harmonic") add_library(UsblTransceiver SHARED src/UsblTransceiver.cc) add_library(UsblTransponder SHARED src/UsblTransponder.cc) add_library(sea_pressure_sensor SHARED src/sea_pressure_sensor.cc) +add_library(UnderwaterCamera SHARED src/UnderwaterCamera.cc) target_include_directories(UsblTransceiver PRIVATE include) target_include_directories(UsblTransponder PRIVATE include) target_include_directories(sea_pressure_sensor PRIVATE include) +target_include_directories(UnderwaterCamera PRIVATE include) target_link_libraries(UsblTransceiver gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} @@ -43,6 +48,11 @@ target_link_libraries(sea_pressure_sensor gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) +target_link_libraries(UnderwaterCamera + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + ${OpenCV_LIBS} +) + # Specify dependencies for FullSystem using ament_target_dependencies ament_target_dependencies(UsblTransceiver dave_interfaces @@ -65,8 +75,13 @@ ament_target_dependencies(sea_pressure_sensor geometry_msgs ) +ament_target_dependencies(UnderwaterCamera + rclcpp + sensor_msgs +) + # Install targets -install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor +install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor UnderwaterCamera DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh new file mode 100644 index 00000000..91725041 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2024 Rakesh Vivekanandan + * + * 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 DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_ +#define DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace dave_gz_sensor_plugins + +{ +class UnderwaterCamera : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate +{ +public: + UnderwaterCamera(); + ~UnderwaterCamera(); + + void Configure( + const gz::sim::Entity & entity, const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override; + + void PostUpdate( + const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override; + + void CameraCallback(const gz::msgs::Image & image); + + void CameraInfoCallback(const gz::msgs::CameraInfo & cameraInfo); + + void DepthImageCallback(const gz::msgs::Image & image); + + cv::Mat ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image); + + cv::Mat SimulateUnderwater( + const cv::Mat & _inputImage, const cv::Mat & _inputDepth, cv::Mat & _outputImage); + +private: + std::shared_ptr ros_node_; + + struct PrivateData; + std::unique_ptr dataPtr; +}; +} // namespace dave_gz_sensor_plugins + +#endif // DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_ diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml index f8e6da97..de9335f2 100644 --- a/gazebo/dave_gz_sensor_plugins/package.xml +++ b/gazebo/dave_gz_sensor_plugins/package.xml @@ -5,6 +5,7 @@ DAVE sensor plugins Gaurav kumar Helena Moyen + Rakesh Vivekanandan Apache 2.0 geometry_msgs ament_cmake @@ -13,6 +14,8 @@ protobuf protobuf dave_interfaces + rclcpp + sensor_msgs ament_cmake diff --git a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc new file mode 100644 index 00000000..230bcfa8 --- /dev/null +++ b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc @@ -0,0 +1,453 @@ +/* + * Copyright (C) 2024 Rakesh Vivekanandan + * + * 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 "dave_gz_sensor_plugins/UnderwaterCamera.hh" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gz/plugin/Register.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/World.hh" + +GZ_ADD_PLUGIN( + dave_gz_sensor_plugins::UnderwaterCamera, gz::sim::System, + dave_gz_sensor_plugins::UnderwaterCamera::ISystemConfigure, + dave_gz_sensor_plugins::UnderwaterCamera::ISystemPostUpdate) + +namespace dave_gz_sensor_plugins +{ + +struct UnderwaterCamera::PrivateData +{ + // Add any private data members here. + gz::sim::Model model; + std::string linkName; + + std::mutex mutex_; + gz::transport::Node gz_node; + std::string topic; + std::string image_topic; + std::string depth_image_topic; + std::string simulated_image_topic; + std::string camera_info_topic; + rclcpp::Publisher::SharedPtr image_pub; + + /// \brief Width of the image. + unsigned int width; + + /// \brief Height of the image. + unsigned int height; + + /// \brief Camera intrinsics. + double fx; + double fy; + double cx; + double cy; + + /// \brief Temporarily store pointer to previous depth image. + gz::msgs::Image lastDepth; + + /// \brief Latest simulated image. + gz::msgs::Image lastImage; + + /// \brief Depth to range lookup table (LUT) + float * depth2rangeLUT; + + /// \brief Attenuation constants per channel (RGB) + float attenuation[3]; + + /// \brief Background constants per channel (RGB) + unsigned char background[3]; + + bool firstImage = true; + + float min_range; + float max_range; +}; + +UnderwaterCamera::UnderwaterCamera() : dataPtr(std::make_unique()) {} + +UnderwaterCamera::~UnderwaterCamera() +{ + if (this->dataPtr->depth2rangeLUT) + { + delete[] this->dataPtr->depth2rangeLUT; + } +} + +void UnderwaterCamera::Configure( + const gz::sim::Entity & _entity, const std::shared_ptr & _sdf, + gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) +{ + gzdbg << "dave_gz_sensor_plugins::UnderwaterCamera::Configure on entity: " << _entity + << std::endl; + + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + + this->ros_node_ = std::make_shared("underwater_camera_node"); + + auto rgbdCamera = _ecm.Component(_entity); + if (!rgbdCamera) + { + gzerr << "UnderwaterCamera plugin should be attached to a rgbd_camera sesnsor. " + << "Failed to initialize." << std::endl; + return; + } + + // get world entity + auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World()); + auto worldName = _ecm.Component(worldEntity)->Data(); + + auto sensorSdf = rgbdCamera->Data(); + this->dataPtr->topic = sensorSdf.Topic(); + + if (sensorSdf.CameraSensor() == nullptr) + { + gzerr << "Attempting to a load an RGBD Camera sensor, but received " + << "a null sensor." << std::endl; + return; + } + + if (this->dataPtr->topic.empty()) + { + auto scoped = gz::sim::scopedName(_entity, _ecm); + this->dataPtr->topic = "/world/" + worldName + "/" + scoped; + } + + this->dataPtr->image_topic = this->dataPtr->topic + "/image"; + this->dataPtr->depth_image_topic = this->dataPtr->topic + "/depth_image"; + this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image"; + + sdf::Camera * cameraSdf = sensorSdf.CameraSensor(); + + // get camera intrinsics + this->dataPtr->width = cameraSdf->ImageWidth(); + this->dataPtr->height = cameraSdf->ImageHeight(); + + if (this->dataPtr->width == 0 || this->dataPtr->height == 0) + { + gzerr << "Camera image size is zero" << std::endl; + return; + } + + this->dataPtr->min_range = cameraSdf->NearClip(); + this->dataPtr->max_range = cameraSdf->FarClip(); + + // Check if min_range and max_range are valid + if (this->dataPtr->min_range == 0 || this->dataPtr->max_range == 0) + { + gzerr << "Invalid min_range or max_range" << std::endl; + return; + } + + this->dataPtr->fx = cameraSdf->LensIntrinsicsFx(); + this->dataPtr->fy = cameraSdf->LensIntrinsicsFy(); + this->dataPtr->cx = cameraSdf->LensIntrinsicsCx(); + this->dataPtr->cy = cameraSdf->LensIntrinsicsCy(); + + // Check if fx and fy are non-zero to avoid division by zero + if (this->dataPtr->fx == 0 || this->dataPtr->fy == 0) + { + gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl; + return; + } + + // Check if cx, cy are zero. If so, set them to the center of the image + if (this->dataPtr->cx == 0) + { + this->dataPtr->cx = this->dataPtr->width / 2; + } + if (this->dataPtr->cy == 0) + { + this->dataPtr->cy = this->dataPtr->height / 2; + } + + gzmsg << "Camera intrinsics: fx=" << this->dataPtr->fx << ", fy=" << this->dataPtr->fy + << ", cx=" << this->dataPtr->cx << ", cy=" << this->dataPtr->cy << std::endl; + + gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height + << std::endl; + + gzmsg << "Min range: " << this->dataPtr->min_range << ", Max range: " << this->dataPtr->max_range + << std::endl; + + // Free previous LUT memory if it was already allocated + if (this->dataPtr->depth2rangeLUT) + { + delete[] this->dataPtr->depth2rangeLUT; + } + + // Allocate memory for the new LUT + this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height]; + float * lutPtr = this->dataPtr->depth2rangeLUT; + + // Fill depth2range LUT + for (int v = 0; v < this->dataPtr->height; v++) + { + double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy; + for (int u = 0; u < this->dataPtr->width; u++) + { + double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx; + // Precompute the per-pixel factor in the following formula: + // range = || (x, y, z) ||_2 + // range = || z * (x/z, y/z, 1.0) ||_2 + // range = z * || (x/z, y/z, 1.0) ||_2 + *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z); + } + } + + if (!_sdf->HasElement("attenuationR")) + { + this->dataPtr->attenuation[0] = 1.f / 30.f; + } + else + { + this->dataPtr->attenuation[0] = _sdf->Get("attenuationR"); + } + + if (!_sdf->HasElement("attenuationG")) + { + this->dataPtr->attenuation[1] = 1.f / 30.f; + } + else + { + this->dataPtr->attenuation[1] = _sdf->Get("attenuationG"); + } + + if (!_sdf->HasElement("attenuationB")) + { + this->dataPtr->attenuation[2] = 1.f / 30.f; + } + else + { + this->dataPtr->attenuation[2] = _sdf->Get("attenuationB"); + } + + if (!_sdf->HasElement("backgroundR")) + { + this->dataPtr->background[0] = (unsigned char)0; + } + else + { + this->dataPtr->background[0] = (unsigned char)_sdf->Get("backgroundR"); + } + + if (!_sdf->HasElement("backgroundG")) + { + this->dataPtr->background[1] = (unsigned char)0; + } + else + { + this->dataPtr->background[1] = (unsigned char)_sdf->Get("backgroundG"); + } + + if (!_sdf->HasElement("backgroundB")) + { + this->dataPtr->background[2] = (unsigned char)0; + } + else + { + this->dataPtr->background[2] = (unsigned char)_sdf->Get("backgroundB"); + } + + // Gazebo camera subscriber + std::function camera_callback = + std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1); + + this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback); + + // Gazebo depth image subscriber + std::function depth_callback = + std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1); + + this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback); + + // ROS2 publisher + this->dataPtr->image_pub = this->ros_node_->create_publisher( + this->dataPtr->simulated_image_topic, 1); +} + +cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image) +{ + int cv_type; + switch (gz_image.pixel_format_type()) + { + case gz::msgs::PixelFormatType::RGB_INT8: + cv_type = CV_8UC3; + break; + case gz::msgs::PixelFormatType::RGBA_INT8: + cv_type = CV_8UC4; + break; + case gz::msgs::PixelFormatType::BGR_INT8: + cv_type = CV_8UC3; + break; + case gz::msgs::PixelFormatType::L_INT8: // MONO8 + cv_type = CV_8UC1; + break; + case gz::msgs::PixelFormatType::R_FLOAT32: // DEPTH32F + cv_type = CV_32FC1; + break; + default: + throw std::runtime_error("Unsupported pixel format"); + } + + // Create OpenCV Mat header that uses the same memory as the Gazebo image data + cv::Mat cv_image( + gz_image.height(), gz_image.width(), cv_type, + const_cast(reinterpret_cast(gz_image.data().data()))); + + // Optionally convert color space if needed (e.g., RGB to BGR) + if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) + { + cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR); + } + else if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8) + { + cv::cvtColor(cv_image, cv_image, cv::COLOR_RGBA2BGRA); + } + + return cv_image; +} + +void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg) +{ + std::lock_guard lock(this->dataPtr->mutex_); + + if (!this->dataPtr->depth2rangeLUT) + { + gzerr << "Depth2range LUT not initialized" << std::endl; + return; + } + else + { + gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraCallback" << std::endl; + + if (this->dataPtr->firstImage) + { + this->dataPtr->lastImage = msg; + this->dataPtr->firstImage = false; + } + else + { + // Convert Gazebo image to OpenCV image + cv::Mat image = this->ConvertGazeboToOpenCV(msg); + + // Convert depth image to OpenCV image using the ConvertGazeboToOpenCV function + cv::Mat depth_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastDepth); + + // Create output image + cv::Mat output_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastImage); + + // Simulate underwater + cv::Mat simulated_image = this->SimulateUnderwater(image, depth_image, output_image); + + // Publish simulated image + sensor_msgs::msg::Image ros_image; + ros_image.header.stamp = this->ros_node_->now(); + ros_image.height = msg.height(); + ros_image.width = msg.width(); + ros_image.encoding = "bgr8"; + ros_image.is_bigendian = false; + ros_image.step = msg.width() * 3; + ros_image.data = std::vector( + simulated_image.data, + simulated_image.data + simulated_image.total() * simulated_image.elemSize()); + + this->dataPtr->image_pub->publish(ros_image); + + // Store the current image for the next iteration + this->dataPtr->lastImage = msg; + } + } +} + +void UnderwaterCamera::DepthImageCallback(const gz::msgs::Image & msg) +{ + std::lock_guard lock(this->dataPtr->mutex_); + + gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::DepthImageCallback" << std::endl; + + this->dataPtr->lastDepth = msg; +} + +cv::Mat UnderwaterCamera::SimulateUnderwater( + const cv::Mat & _inputImage, const cv::Mat & _inputDepth, cv::Mat & _outputImage) +{ + const float * lutPtr = this->dataPtr->depth2rangeLUT; + for (unsigned int row = 0; row < this->dataPtr->height; row++) + { + const cv::Vec3b * inrow = _inputImage.ptr(row); + const float * depthrow = _inputDepth.ptr(row); + cv::Vec3b * outrow = _outputImage.ptr(row); + + for (int col = 0; col < this->dataPtr->width; col++) + { + // Convert depth to range using the depth2range LUT + float r = *(lutPtr++) * depthrow[col]; + + const cv::Vec3b & in = inrow[col]; + cv::Vec3b & out = outrow[col]; + + if (r < this->dataPtr->min_range) + { + r = this->dataPtr->min_range; + } + else if (r > this->dataPtr->max_range) + { + r = this->dataPtr->max_range; + } + + for (int c = 0; c < 3; c++) + { + // Simplifying assumption: intensity ~ irradiance. + // This is not really the case but a good enough approximation + // for now (it would be better to use a proper Radiometric + // Response Function). + float e = std::exp(-r * this->dataPtr->attenuation[c]); + out[c] = e * in[c] + (1.0f - e) * this->dataPtr->background[c]; + } + } + } + return _outputImage; +} + +void UnderwaterCamera::PostUpdate( + const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm) +{ + if (!_info.paused) + { + rclcpp::spin_some(this->ros_node_); + + if (_info.iterations % 1000 == 0) + { + gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::PostUpdate" << std::endl; + } + } +} + +} // namespace dave_gz_sensor_plugins diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc index eccb40d5..df18db0f 100644 --- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc +++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc @@ -43,6 +43,7 @@ struct SubseaPressureSensorPlugin::PrivateData rclcpp::Publisher::SharedPtr ros_depth_estimate_pub; std::shared_ptr rosNode; std::string robotNamespace; + std::string topic; double noiseAmp = 0.0; double noiseSigma = 3.0; double inferredDepth = 0.0; @@ -83,6 +84,15 @@ void SubseaPressureSensorPlugin::Configure( } this->dataPtr->robotNamespace = _sdf->Get("namespace"); + if (_sdf->HasElement("topic")) + { + this->dataPtr->topic = _sdf->Get("topic"); + } + else + { + this->dataPtr->topic = "sea_pressure"; + } + if (_sdf->HasElement("saturation")) { this->dataPtr->saturation = _sdf->Get("saturation"); @@ -124,18 +134,20 @@ void SubseaPressureSensorPlugin::Configure( this->dataPtr->ros_pressure_sensor_pub = this->rosNode->create_publisher( - this->dataPtr->robotNamespace + "/" + "Pressure", rclcpp::QoS(10).reliable()); + "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic, + rclcpp::QoS(10).reliable()); if (this->dataPtr->estimateDepth) { this->dataPtr->ros_depth_estimate_pub = this->rosNode->create_publisher( - this->dataPtr->robotNamespace + "/" + "Pressure_depth", rclcpp::QoS(10).reliable()); + "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic + "_depth", + rclcpp::QoS(10).reliable()); } this->dataPtr->gz_pressure_sensor_pub = this->dataPtr->gazeboNode->Advertise( - this->dataPtr->robotNamespace + "/" + "Pressure"); + "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic); } ////////////////////////////////////////// @@ -242,4 +254,4 @@ void SubseaPressureSensorPlugin::PostUpdate( } } -} // namespace dave_gz_sensor_plugins \ No newline at end of file +} // namespace dave_gz_sensor_plugins diff --git a/models/dave_robot_models/description/glider_slocum/model.sdf b/models/dave_robot_models/description/glider_slocum/model.sdf index 223c30a6..ac542216 100644 --- a/models/dave_robot_models/description/glider_slocum/model.sdf +++ b/models/dave_robot_models/description/glider_slocum/model.sdf @@ -39,15 +39,34 @@ + + + + 0 0 0 0 0 0 + 0.015 + + 0.00001 + 0.0 + 0.0 + 0.00001 + 0.0 + 0.00001 + + true - 1000.0 - imu + 50.0 + /model/glider_slocum/imu - + + 0 0 0 0 0 0 + base_link + imu_link + + 0 0 0 0 0 0 @@ -88,6 +107,33 @@ true + + + 0 0 0 0 0 0 + 0.001 + + 0.00001 + 0.0 + 0.0 + 0.00001 + 0.0 + 0.00001 + + + + + true + 30.0 + /model/glider_slocum/navsat + + + + + 0 0 0 0 0 0 + base_link + navsat_link + + 0 0 0 0 0 0 @@ -179,6 +225,11 @@ name="gz::sim::systems::Imu"> + + + @@ -193,6 +244,18 @@ name="gz::sim::systems::JointStatePublisher"> + + + glider_slocum + sea_pressure + 10 + 3.0 + true + 101.325 + 9.80638 + diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf index 4e4e5d61..4d383d2c 100644 --- a/models/dave_robot_models/description/rexrov/model.sdf +++ b/models/dave_robot_models/description/rexrov/model.sdf @@ -64,15 +64,241 @@ + + + + 0 0 0 0 0 0 + + 0.015 + + 0.00001 + 0.0 + 0.0 + 0.00001 + 0.0 + 0.00001 + + + + true + 50.0 + /model/rexrov/magnetometer + + + + 0.0 + 1.0 + + + + + 0.0 + 1.0 + + + + + 0.0 + 1.4 + + + + + + + + 0 0 0 0 0 0 + base_link + magnetometer_link + + + + + 0 0 0 0 0 0 + 0.015 + + 0.00001 + 0.0 + 0.0 + 0.00001 + 0.0 + 0.00001 + + true - 1000.0 - imu + 50.0 + /model/rexrov/imu + + + + + 0 0 0 0 0 0 + base_link + imu_link + + + + -1.34 0 -0.75 0 0 -1.57 + + 3.5 + + 0.0195872 + 0 + 0 + 0.0195872 + 0 + 0.0151357 + + + + 1 + + + + 0 0 0 0 0 0 + 1 + 8 + /model/rexrov/dvl/velocity + + phased_array + + + 3 + -135 + 25 + + + 3 + 135 + 25 + + + 3 + 45 + 25 + + + 3 + -45 + 25 + + + + + best + + 0. + 0. + 0. + + + 10. + 100. + + 10 + + 0.015 + + true + + + best + + 0.005 + + true + + + 200. + 0.3 + + 0 0 0 3.14 0 -1.57 + + 0 + 0 + 0 + + 0 0 0 0 0 3.14159 + + + 1 1 1 + + model://meshes/nortek_dvl500_300/DVL500-300m.dae + + + 0 + 1 + + + 0 + 10 + 0 0 0 3.14159 0 0 + + + 0.093 + 0.203 + + + + + + 1 + 1 + 0 0 0 + 0 + 0 + + + 1 + 0 + 0 + 1 + + 0 + + + + + 0 + 1e+06 + + + 0 + 1 + 1 + + 0 + 0.2 + 1e+13 + 1 + 0.01 + 0 + + + 1 + -0.01 + 0 + 0.2 + 1e+13 + 1 + + + + + + -1.34 0 -0.75 0 0 -1.57 + base_link + dvl_link + + -0.890895 0.334385 0.528822 0 -1.300794 -0.928689 @@ -562,6 +788,11 @@ name="gz::sim::systems::Imu"> + + + @@ -574,14 +805,6 @@ - thruster1_joint - thruster2_joint - thruster3_joint - thruster4_joint - thruster5_joint - thruster6_joint - thruster7_joint - thruster8_joint @@ -597,5 +820,11 @@ 9.80638 + + /model/rexrov/dvl/velocity + + - \ No newline at end of file + diff --git a/models/dave_robot_models/launch/glider_slocum.launch.py b/models/dave_robot_models/launch/glider_slocum.launch.py new file mode 100644 index 00000000..59ba3060 --- /dev/null +++ b/models/dave_robot_models/launch/glider_slocum.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + glider_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=[ + "/model/glider_slocum/battery/battery/state@sensor_msgs/msg/BatteryState@gz.msgs.BatteryState", + "/model/glider_slocum/joint/propeller_joint/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double", + "/model/glider_slocum/joint/propeller_joint/ang_vel@std_msgs/msg/Float64@gz.msgs.Double", + "/model/glider_slocum/joint/propeller_joint/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean", + "/model/glider_slocum/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry", + "/model/glider_slocum/odometry_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance", + "/model/glider_slocum/pose@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V", + "/model/glider_slocum/navsat@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat", + "/model/glider_slocum/imu@sensor_msgs/msg/Imu@gz.msgs.IMU", + ], + output="screen", + ) + + return LaunchDescription([glider_bridge]) diff --git a/models/dave_robot_models/launch/rexrov.launch.py b/models/dave_robot_models/launch/rexrov.launch.py new file mode 100644 index 00000000..39e00c0d --- /dev/null +++ b/models/dave_robot_models/launch/rexrov.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + thruster_joints = [ + "/model/rexrov/joint/thruster1_joint", + "/model/rexrov/joint/thruster2_joint", + "/model/rexrov/joint/thruster3_joint", + "/model/rexrov/joint/thruster4_joint", + "/model/rexrov/joint/thruster5_joint", + "/model/rexrov/joint/thruster6_joint", + "/model/rexrov/joint/thruster7_joint", + "/model/rexrov/joint/thruster8_joint", + ] + + rexrov_arguments = ( + [f"{joint}/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints] + + [f"{joint}/ang_vel@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints] + + [ + f"{joint}/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean" + for joint in thruster_joints + ] + + [ + "/model/rexrov/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry", + "/model/rexrov/odometry_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance", + "/model/rexrov/pose@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V", + "/model/rexrov/imu@sensor_msgs/msg/Imu@gz.msgs.IMU", + "/model/rexrov/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer", + ] + ) + + rexrov_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=rexrov_arguments, + output="screen", + ) + + return LaunchDescription([rexrov_bridge]) diff --git a/models/dave_robot_models/launch/upload_robot.launch.py b/models/dave_robot_models/launch/upload_robot.launch.py index 5b499add..b22c0543 100755 --- a/models/dave_robot_models/launch/upload_robot.launch.py +++ b/models/dave_robot_models/launch/upload_robot.launch.py @@ -1,10 +1,20 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, LogInfo -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import ( + DeclareLaunchArgument, + RegisterEventHandler, + LogInfo, + IncludeLaunchDescription, +) +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + EqualsSubstitution, +) from launch.conditions import IfCondition from launch_ros.substitutions import FindPackageShare from launch.event_handlers import OnProcessExit from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): @@ -128,10 +138,36 @@ def generate_launch_description(): nodes = [tf2_spawner, gz_spawner] + # Include the Rexrov launch file if the namespace is "rexrov" + rexrov_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("dave_robot_models"), "launch", "rexrov.launch.py"] + ) + ), + condition=IfCondition(EqualsSubstitution(namespace, "rexrov")), + ) + + # Include the Glider launch file if the namespace is "glider_slocum" + glider_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("dave_robot_models"), + "launch", + "glider_slocum.launch.py", + ] + ) + ), + condition=IfCondition(EqualsSubstitution(namespace, "glider_slocum")), + ) + + include = [rexrov_launch, glider_launch] + event_handlers = [ RegisterEventHandler( OnProcessExit(target_action=gz_spawner, on_exit=LogInfo(msg="Robot Model Uploaded")) ) ] - return LaunchDescription(args + nodes + event_handlers) + return LaunchDescription(args + nodes + event_handlers + include) diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf new file mode 100644 index 00000000..03d594ee --- /dev/null +++ b/models/dave_sensor_models/description/underwater_camera/model.sdf @@ -0,0 +1,60 @@ + + + + + + 0 0 0 0 0 0 + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 10 + true + 1 + underwater_camera + + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + 0.8 + 0.5 + 0.2 + + 85 + 107 + 47 + + + + true + + + diff --git a/models/dave_worlds/worlds/new_dvl.world b/models/dave_worlds/worlds/camera_tutorial.world similarity index 76% rename from models/dave_worlds/worlds/new_dvl.world rename to models/dave_worlds/worlds/camera_tutorial.world index 0d39c147..3b4be54e 100644 --- a/models/dave_worlds/worlds/new_dvl.world +++ b/models/dave_worlds/worlds/camera_tutorial.world @@ -14,142 +14,84 @@ See the License for the specific language governing permissions and limitations under the License. --> + - - - - 0.0 1.0 1.0 - - 0.0 0.7 0.8 - - false + + + 0.01 0.01 0.01 1.0 + + + 12 + + + 1 - - - 0.001 - 1.0 - - - - - - - - - + + 0.001 + 1.0 + + + + + + + + ogre2 - - - - 56.71897669633431 - 3.515625 - - - - - - - 50 0 150 0 0 0 - 1 1 1 1 - 0.1 0.1 0.1 1 - 0.3 0.3 -1 - false - - - - 20 0 -90 0 0 0 - 0.9 0.9 0.9 1 - 0.2 0.2 0.2 1 - -1 0 0 - false - - - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame - - 0 0 0 0 0 0 - - - - - - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Waves - - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap - 0 0 -100 0 0 0 - - - - true - 21 -1 -98.5 0 0 0 - - - - 1 - 0 - 0 - 1 - 0 - 1 - - 1.0 - - - - - 0.03 - 3.0 - - - - - - - 0.03 - 3.0 - - - - - - - - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase with Inertia - 20 0 -98 0 0 0 - - - - https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase Distorted - 22 -0.5 -100 0 0 1.5708 - - - + + + 56.71897669633431 + 3.515625 + + + + + 50 0 150 0 0 0 + 1 1 1 1 + .1 .1 .1 1 + 0.3 0.3 -1 + false + + + + -50 0 -150 0 0 0 + 0.6 0.6 0.6 1 + 0 0 0 1 + -0.3 -0.3 -1 + false + + + + -100 500 -20 0 0 0 + 0.8 0.8 0.8 1 + 1 1 1 1 + -1 -1 0 + false + + + + -150 -130 50 0 0 0 + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + 0.5 0.5 -1 + false + + + @@ -162,7 +104,7 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - 18.7 3.77 -97.5 0 0.29 -0.715 + 7.5 5.0 -90 0 0.40 -1.57 0.25 2500 @@ -395,8 +337,52 @@ + + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame + + 0 0 0 0 0 0 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Coast Water + + + + + + https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap + 0 0 -95 0 0 0 + + + + sunken_vase + 7.5 0 -95 0 0 0 + https://fuel.ignitionrobotics.org/1.0/Cole/models/Sunken Vase + + + + sunken_vase_distorted + 7.5 1 -95 0 0 0 + https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase Distorted + true + + + + Coral1 + 7 -1 -95 0 0 0 + https://fuel.ignitionrobotics.org/1.0/Cole/models/Coral01 + true + + @@ -409,5 +395,5 @@ - + diff --git a/models/dave_worlds/worlds/dave_Santorini.world b/models/dave_worlds/worlds/dave_Santorini.world index 7d172e8a..f6704ceb 100644 --- a/models/dave_worlds/worlds/dave_Santorini.world +++ b/models/dave_worlds/worlds/dave_Santorini.world @@ -43,6 +43,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + diff --git a/models/dave_worlds/worlds/dave_bimanual_example.world b/models/dave_worlds/worlds/dave_bimanual_example.world index 56287925..b6cc0cc2 100644 --- a/models/dave_worlds/worlds/dave_bimanual_example.world +++ b/models/dave_worlds/worlds/dave_bimanual_example.world @@ -36,6 +36,14 @@ + + + + 56.71897669633431 diff --git a/models/dave_worlds/worlds/dave_electrical_mating.world b/models/dave_worlds/worlds/dave_electrical_mating.world index 18434f24..b724b5a2 100644 --- a/models/dave_worlds/worlds/dave_electrical_mating.world +++ b/models/dave_worlds/worlds/dave_electrical_mating.world @@ -34,6 +34,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + diff --git a/models/dave_worlds/worlds/dave_graded_seabed.world b/models/dave_worlds/worlds/dave_graded_seabed.world index 1b4690a8..557573bb 100644 --- a/models/dave_worlds/worlds/dave_graded_seabed.world +++ b/models/dave_worlds/worlds/dave_graded_seabed.world @@ -41,6 +41,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + diff --git a/models/dave_worlds/worlds/dave_integrated.world b/models/dave_worlds/worlds/dave_integrated.world index dde9496d..c4fccf98 100644 --- a/models/dave_worlds/worlds/dave_integrated.world +++ b/models/dave_worlds/worlds/dave_integrated.world @@ -36,7 +36,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> - + + + + diff --git a/models/dave_worlds/worlds/dave_ocean_models.world b/models/dave_worlds/worlds/dave_ocean_models.world index c1c293e0..724914e7 100644 --- a/models/dave_worlds/worlds/dave_ocean_models.world +++ b/models/dave_worlds/worlds/dave_ocean_models.world @@ -43,6 +43,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + diff --git a/models/dave_worlds/worlds/dave_ocean_waves.world b/models/dave_worlds/worlds/dave_ocean_waves.world index 920c9f59..e1038384 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves.world +++ b/models/dave_worlds/worlds/dave_ocean_waves.world @@ -43,6 +43,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + diff --git a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world index 88beb352..8bccf334 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world +++ b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world @@ -34,6 +34,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + + 0.01 0.01 0.01 1.0 diff --git a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world index 1fd5c19f..215c309b 100644 --- a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world +++ b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world @@ -43,7 +43,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> - + + + + 56.71897669633431 diff --git a/models/dave_worlds/worlds/dave_plug_and_socket.world b/models/dave_worlds/worlds/dave_plug_and_socket.world index bdb3b1f5..c380a757 100644 --- a/models/dave_worlds/worlds/dave_plug_and_socket.world +++ b/models/dave_worlds/worlds/dave_plug_and_socket.world @@ -34,6 +34,14 @@ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> + + + +