From 4b4b0bd995d85396e8bb7e281c51467612d4349e Mon Sep 17 00:00:00 2001 From: Keith Khadar Date: Sat, 11 Jan 2025 02:31:01 +0000 Subject: [PATCH] began work on other plugins --- .../config/subjugator_bridge.yaml | 7 +- .../urdf/sub9.urdf.xacro | 21 +++++ .../urdf/xacro/camera.xacro | 2 +- .../urdf/xacro/dvl.xacro | 82 +++++++++++++------ .../urdf/xacro/imu_magnetometer.xacro | 71 ++++++++++++++++ .../UnderwaterCamera.hh | 3 + .../subjugator_gazebo/src/UnderwaterCamera.cc | 24 +++++- .../worlds/robosub_2024.world | 6 +- 8 files changed, 182 insertions(+), 34 deletions(-) create mode 100644 src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro rename src/subjugator/simulation/subjugator_gazebo/include/{dave_gz_sensor_plugins => }/UnderwaterCamera.hh (97%) diff --git a/src/subjugator/simulation/subjugator_bringup/config/subjugator_bridge.yaml b/src/subjugator/simulation/subjugator_bringup/config/subjugator_bridge.yaml index fed7bad..9d49b82 100644 --- a/src/subjugator/simulation/subjugator_bringup/config/subjugator_bridge.yaml +++ b/src/subjugator/simulation/subjugator_bringup/config/subjugator_bridge.yaml @@ -4,4 +4,9 @@ # ros_type: "std_msgs/String" # gz_type: "gz.msgs.StringMsg" -[] +# IMU configuration. +- topic_name: "/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + lazy: true + direction: GZ_TO_ROS \ No newline at end of file diff --git a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro index 0a351f8..4556f2e 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro @@ -4,6 +4,9 @@ + + + @@ -66,4 +69,22 @@ + + + + + diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro index a04558b..164f995 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro @@ -21,7 +21,7 @@ ${fps} true 1 - ${namespace}/image_raw + ${namespace} ${fov} diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro index 36c6551..003d5c4 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro @@ -1,7 +1,7 @@ - + - + @@ -9,28 +9,60 @@ - - true - ${rate} - - - - 1 - 3 - -0.3 - 0.3 - - - - 0.05 - 200 - - - - ${name} - ${gazebo_offset} - - + + + 0 0 0 0 0 0 + 1 + ${rate} + /model/${parent}/dvl/velocity + + phased_array + + + 2 + 45 + 30 + + + 2 + 135 + 30 + + + 2 + -45 + 30 + + + 2 + -135 + 30 + + + + + best + + + 0.002 + + false + + + + 0.01 + 100. + 0.1 + + 0 0 0 0 0 -1.570796 + + + + - \ No newline at end of file + diff --git a/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro b/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro new file mode 100644 index 0000000..9e50154 --- /dev/null +++ b/src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro @@ -0,0 +1,71 @@ + + + + + + + + + + + + true + ${rate} + ${imu_topic}/imu + + + + + ${lx} + + + + + ${ly} + + + + + ${lz} + + + + + + + ${ax} + + + + + ${ay} + + + + + ${az} + + + + + + + + + + true + ${rate} + ${imu_topic}/imu_mag + + + + + + \ No newline at end of file diff --git a/src/subjugator/simulation/subjugator_gazebo/include/dave_gz_sensor_plugins/UnderwaterCamera.hh b/src/subjugator/simulation/subjugator_gazebo/include/UnderwaterCamera.hh similarity index 97% rename from src/subjugator/simulation/subjugator_gazebo/include/dave_gz_sensor_plugins/UnderwaterCamera.hh rename to src/subjugator/simulation/subjugator_gazebo/include/UnderwaterCamera.hh index e068578..8410fed 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/dave_gz_sensor_plugins/UnderwaterCamera.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/UnderwaterCamera.hh @@ -63,6 +63,9 @@ private: struct PrivateData; std::unique_ptr dataPtr; }; + + std::string sanitizeNodeName(const std::string& name); + } // namespace dave_gz_sensor_plugins #endif // DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_ \ No newline at end of file diff --git a/src/subjugator/simulation/subjugator_gazebo/src/UnderwaterCamera.cc b/src/subjugator/simulation/subjugator_gazebo/src/UnderwaterCamera.cc index 3309536..7259e9c 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/UnderwaterCamera.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/UnderwaterCamera.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "dave_gz_sensor_plugins/UnderwaterCamera.hh" +#include "UnderwaterCamera.hh" #include #include #include @@ -109,7 +109,6 @@ void UnderwaterCamera::Configure( rclcpp::init(0, nullptr); } - this->ros_node_ = std::make_shared("underwater_camera_node"); auto rgbdCamera = _ecm.Component(_entity); if (!rgbdCamera) @@ -143,6 +142,9 @@ void UnderwaterCamera::Configure( this->dataPtr->depth_image_topic = this->dataPtr->topic + "/depth_image"; this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image"; + std::string validName = sanitizeNodeName(this->dataPtr->topic + "_node"); + this->ros_node_ = std::make_shared(validName); + sdf::Camera * cameraSdf = sensorSdf.CameraSensor(); // get camera intrinsics @@ -450,4 +452,22 @@ void UnderwaterCamera::PostUpdate( } } +// Function to sanitize a ROS node name +std::string sanitizeNodeName(const std::string& name) { + std::string sanitized; + for (char c : name) { + // Replace invalid characters with '_' + if (std::isalnum(c) || c == '_') { + sanitized += c; + } else { + sanitized += '_'; + } + } + // Ensure the name doesn't start with a digit + if (!sanitized.empty() && std::isdigit(sanitized[0])) { + sanitized = "_" + sanitized; + } + return sanitized; +} + } // namespace dave_gz_sensor_plugins \ No newline at end of file diff --git a/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2024.world b/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2024.world index a675ded..0787178 100644 --- a/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2024.world +++ b/src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2024.world @@ -18,7 +18,7 @@ 100 - 0.001 + 0.01 1.0 @@ -42,10 +42,6 @@ name="gz::sim::systems::Sensors"> ogre2 - -