Carter torpedo plugin

src/subjugator/command/subjugator_keyboard_control/include/subjugator_keyboard_control/SubjugatorKeyboardControl.h

@@ -12,6 +12,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "geometry_msgs/msg/wrench.hpp"
+#include "std_msgs/msg/string.hpp"
 
 // May be used elsewhere if similar logic?
 struct KeyState
@@ -35,6 +36,8 @@ class SubjugatorKeyboardControl final : public rclcpp::Node
 
   private:
     rclcpp::Publisher<geometry_msgs::msg::Wrench>::SharedPtr publisher_;
+    // Add publisher for keypress events
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr keypress_publisher_;
     std::atomic<double> force_x_, force_y_, force_z_;
     std::atomic<double> torque_x_, torque_y_, torque_z_;
     double base_linear_, base_angular_;

src/subjugator/command/subjugator_keyboard_control/src/SubjugatorKeyboardControl.cpp

@@ -21,6 +21,8 @@ SubjugatorKeyboardControl::SubjugatorKeyboardControl()
   , running_(true)
 {
     publisher_ = this->create_publisher<geometry_msgs::msg::Wrench>("cmd_wrench", PUBLISH_RATE);
+    // Add publisher for keypress events
+    keypress_publisher_ = this->create_publisher<std_msgs::msg::String>("/keyboard/keypress", 10);
     this->declare_parameter("linear_speed", 100.0);
     this->declare_parameter("angular_speed", 100.0);
     base_linear_ = this->get_parameter("linear_speed").as_double();
@@ -47,6 +49,7 @@ SubjugatorKeyboardControl::SubjugatorKeyboardControl()
         Left        : yaw left   (+torque z)
         e           : roll left  (-torque x)
         r           : roll right (+torque x)
+        t           : Launch torpedo (2 Available)
         q           : Quit
     )");
 }
@@ -196,6 +199,13 @@ void SubjugatorKeyboardControl::keyboardLoop()
                         key_x.pressed = true;
                         key_x.last_time = now;
                         break;
+                    case 't':
+                    {
+                        std_msgs::msg::String keypress_msg;
+                        keypress_msg.data = "t";
+                        keypress_publisher_->publish(keypress_msg);
+                        break;
+                    }
                     case 'q':
                         running_ = false;
                         rclcpp::shutdown();

src/subjugator/simulation/subjugator_description/models/torpedo/torpedo.sdf

@@ -3,9 +3,10 @@
   <model name="torpedo">
     <static>false</static>
     <link name="body">
+
       <!-- Change this to reflect real values -->
       <inertial>
-        <mass>.022</mass>
+        <mass>.0156</mass>
         <inertia>
           <ixx>1.18</ixx>
           <ixy>-0.003</ixy>
@@ -20,15 +21,19 @@
         <linear>0.00001</linear>
         <angular>0.00001</angular>
       </velocity_decay>
-      <collision name='bodycol'>
+
+      <!-- The collision geometry -->
+      <collision name="bodycol">
         <pose>0 0 0 0 1.57 0</pose>
         <geometry>
           <cylinder>
-            <radius>.01</radius>
-            <length>.125</length>
+            <radius>0.1</radius>
+            <length>0.25</length>
           </cylinder>
         </geometry>
       </collision>
+
+      <!-- The physical aspects of the torpedo -->
       <visual name="bodyvisual">
         <pose>0 0 0 0 1.57 0</pose>
         <geometry>
@@ -47,22 +52,11 @@
           <collision>bodycol</collision>
         </contact>
         <update_rate>50</update_rate>
-        <plugin name='gazebo_ros_bumper_controller' filename="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <bumperTopicName>contact_bumper</bumperTopicName>
-        </plugin>
       </sensor>
-    </link>
-
-    <plugin name="torpedo_boyancy" filename="libsubjugator_buoyancy.so">
-      <fluid_density>1000</fluid_density>
-      <drag_coefficient>100</drag_coefficient>
-      <link name="body">
-        <center_of_volume>0 0 0</center_of_volume>
-        <volume>0.0000156</volume>
-      </link>
-    </plugin>
-
 
+      <!-- Add center_of_volume and volume for buoyancy system plugin in world file -->
+      <center_of_volume>0 0 0</center_of_volume>
+      <volume>0.0000156</volume>
+    </link>
   </model>
 </sdf>

src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro

@@ -95,6 +95,8 @@
       <nRabsR>-750</nRabsR>
       <nR>-750</nR>
     </plugin>
+
+    <plugin filename="Torpedoes" name="torpedoes::Torpedoes"/>
   </gazebo>
 
   <!-- Cameras -->

src/subjugator/simulation/subjugator_description/urdf/xacro/hydrophone.xacro

@@ -24,13 +24,5 @@
       <disableFixedJointLumping>true</disableFixedJointLumping>
       <preserveFixedJoint>true</preserveFixedJoint>
     </gazebo>
-
-    <!-- <gazebo>
-      <plugin
-        filename="Hydrophone"
-        name="hydrophone::Hydrophone">
-        <topic>${name}</topic>
-      </plugin>
-    </gazebo> -->
   </xacro:macro>
 </robot>

src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(mil_msgs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -52,6 +53,9 @@ add_library(DepthSensor SHARED src/DepthSensor.cc)
 # Plugin 3: Hydrophone
 add_library(Hydrophone SHARED src/Hydrophone.cc)
 
+# Plugin 4: Torpedoes
+add_library(Torpedoes SHARED src/Torpedoes.cc)
+
 # Ensures plugin can see include directory to access its header files
 target_include_directories(ExamplePlugin PRIVATE include)
 
@@ -63,6 +67,8 @@ target_include_directories(DepthSensor PRIVATE include)
 
 target_include_directories(Hydrophone PRIVATE include)
 
+target_include_directories(Torpedoes PRIVATE include)
+
 # Add additional needed libraries
 target_link_libraries(
   ExamplePlugin gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} # Links gazebo
@@ -83,6 +89,10 @@ target_link_libraries(Hydrophone gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
 # Link libraries for DepthSensor plugin
 target_link_libraries(DepthSensor gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
 
+# Link libraries for Torpedoes plugin
+target_link_libraries(Torpedoes gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
+target_link_libraries(Torpedoes ament_index_cpp::ament_index_cpp)
+
 # Specify dependencies using ament_target_dependencies
 ament_target_dependencies(
   ExamplePlugin
@@ -99,9 +109,11 @@ ament_target_dependencies(DepthSensor rclcpp mil_msgs sensor_msgs)
 
 ament_target_dependencies(Hydrophone rclcpp geometry_msgs mil_msgs)
 
+ament_target_dependencies(Torpedoes rclcpp geometry_msgs mil_msgs)
+
 # Install the plugins
 install(TARGETS UnderwaterCamera ExamplePlugin DVLBridge Hydrophone DepthSensor
-        LIBRARY DESTINATION lib/${PROJECT_NAME})
+                Torpedoes LIBRARY DESTINATION lib/${PROJECT_NAME})
 
 # Install headers
 install(DIRECTORY include/ DESTINATION include)

src/subjugator/simulation/subjugator_gazebo/include/Torpedoes.hh

@@ -0,0 +1,97 @@
+#ifndef TORPEDOES__HH_
+#define TORPEDOES__HH_
+
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <set>
+#include <sstream>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>  // For ROS2
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <gz/plugin/Register.hh>  // For GZ_ADD_PLUGIN
+// 'gz' and 'sdf' includes turn into namespace::namespace::ClassName
+#include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_factory.pb.h>
+
+#include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/System.hh>
+#include <gz/sim/components/LinearVelocity.hh>
+#include <gz/sim/components/Link.hh>
+#include <gz/sim/components/Name.hh>
+#include <gz/sim/components/Pose.hh>
+#include <gz/sim/components/World.hh>
+#include <gz/transport/Node.hh>
+#include <sdf/Element.hh>
+#include <std_msgs/msg/string.hpp>
+
+namespace torpedoes
+{
+class Torpedoes : public gz::sim::System,
+                  public gz::sim::ISystemConfigure,
+                  public gz::sim::ISystemPreUpdate,
+                  public gz::sim::ISystemPostUpdate
+{
+  public:
+    Torpedoes();
+    ~Torpedoes();
+
+    // Configure() - Gathers Info at the start of the simulation //
+    void Configure(gz::sim::Entity const &entity, std::shared_ptr<sdf::Element const> const &sdf,
+                   gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) override;
+
+    // System PreUpdate - Called every simulation step before physics //
+    void PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &ecm) override;
+
+    // System PostUpdate - Called every simulation step to update pinger/torpedoes distances //
+    void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override;
+
+    // Callback for keypress
+    void KeypressCallback(std_msgs::msg::String::SharedPtr const msg);
+
+    // SpawnTorpedo - Spawns a torpedo in the simulation //
+    void SpawnTorpedo(std::string const &worldName, std::string const &sdfPath);
+
+  private:
+    // ROS2 Node and Subscription for keypress //
+    rclcpp::Node::SharedPtr torpedo_node_;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr keypress_sub_;
+
+    // Sub9 SDF & Entity //
+    std::shared_ptr<sdf::Element const> sub9_SDF;
+    gz::sim::Entity sub9_Entity{ gz::sim::kNullEntity };
+    gz::math::Pose3d sub9_pose;
+    bool subEntityFound = false;  // Track if the sub9 entity has been found
+
+    // Torpedo Data Structures & Variables //
+    std::set<std::string> torpedoModelNames;          // Unique names of torpedo models spawned
+    std::set<std::string> torpedoesWithVelocitySet;   // Names of torpedoes that have had their velocity set
+    std::map<std::string, double> torpedoSpawnTimes;  // Track spawn times of torpedoes
+    int torpedoCount = 0;
+    bool t_pressed = false;  // Track if 't' key was pressed
+    bool worldNameFound = false;
+    std::string worldName = "bingbong";
+
+    // Torpedo SDF Values //
+    unsigned int timeout = 2000;  // Timeout in milliseconds
+    gz::msgs::Boolean reply;
+    bool result = false;
+    gz::transport::Node node;
+    std::string const Torpedo_sdfPath = ament_index_cpp::get_package_share_directory("subjugator_description") + "/mode"
+                                                                                                                 "ls/"
+                                                                                                                 "torpe"
+                                                                                                                 "do/"
+                                                                                                                 "torpe"
+                                                                                                                 "do."
+                                                                                                                 "sdf";
+    // Pre-commit forces me to do this ugly sdfPath thingy above :(
+};
+
+}  // namespace torpedoes
+#endif  // TORPEDOES_HH