From 915a9db17b1e825700b38aa0b8e90624358fef06 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 23 Mar 2026 12:53:09 -0400 Subject: [PATCH 01/11] Just some comments --- src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc index 8c246a02..f8709731 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc @@ -32,6 +32,7 @@ void MarbleDropper::Configure(gz::sim::Entity const &entity, std::shared_ptrsub9_SDF = sdf->Clone(); + // Get the world name from the EntityComponentManager (only done once) if (!worldNameFound) { ecm.Each( From c0084385257d81dbbdccc07af438339e6db028b7 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 23 Mar 2026 12:55:09 -0400 Subject: [PATCH 02/11] Merged main/Gripper into this branch --- .../simulation/subjugator_gazebo/src/GripperControl.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc index 0809152d..0e52ab10 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc @@ -31,7 +31,6 @@ void GripperControl::Configure(gz::sim::Entity const &entity, std::shared_ptrnode_ = std::make_shared("gripper_control_plugin_node"); this->key_sub_ = this->node_->create_subscription( "/keyboard/keypress", 10, std::bind(&GripperControl::KeypressCallback, this, std::placeholders::_1)); - std::cout << "[GripperControl] ROS2 node created and subscribed to /keyboard/keypress" << std::endl; // Try to obtain model name (from Entity Name component or SDF attribute) From e1952f76a3715ce1a97b404fb814bdb0af9e5da2 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 23 Mar 2026 16:17:19 -0400 Subject: [PATCH 03/11] I wrote the code for the Gripper Server side of the Gripper service setup. Next is to write the client, test it, and then implement similar services for the other servos --- .../package.xml | 18 ++++++++ .../resource/subjugator_sim_actuator_client | 0 .../subjugator_sim_actuator_client/setup.cfg | 4 ++ .../subjugator_sim_actuator_client/setup.py | 27 ++++++++++++ .../__init__.py | 0 .../actuator_client.py | 0 .../subjugator_gazebo/CMakeLists.txt | 4 +- .../include/GripperControl.hh | 19 ++++++--- .../subjugator_gazebo/src/GripperControl.cc | 41 +++++++++++++------ 9 files changed, 92 insertions(+), 21 deletions(-) create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/package.xml create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/resource/subjugator_sim_actuator_client create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/setup.cfg create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/setup.py create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/__init__.py create mode 100644 src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml b/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml new file mode 100644 index 00000000..b2fc3ff3 --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml @@ -0,0 +1,18 @@ + + + + subjugator_sim_actuator_client + 0.0.0 + TODO: Package description + ckreis + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/resource/subjugator_sim_actuator_client b/src/subjugator/gnc/subjugator_sim_actuator_client/resource/subjugator_sim_actuator_client new file mode 100644 index 00000000..e69de29b diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.cfg b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.cfg new file mode 100644 index 00000000..265de056 --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/subjugator_sim_actuator_client +[install] +install_scripts=$base/lib/subjugator_sim_actuator_client diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py new file mode 100644 index 00000000..b4ac42cd --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = "subjugator_sim_actuator_client" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="ckreis", + maintainer_email="cckreis0507@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + extras_require={ + "test": [ + "pytest", + ], + }, + entry_points={ + "console_scripts": [], + }, +) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/__init__.py b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt index c7b3bb57..022f693e 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -5,6 +5,7 @@ project(subjugator_gazebo) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -128,8 +129,7 @@ ament_target_dependencies(ThrusterBridge rclcpp subjugator_msgs geometry_msgs) ament_target_dependencies(MarbleDropper rclcpp subjugator_msgs geometry_msgs) -# ROS2 / ament dependencies used by your plugin -ament_target_dependencies(GripperControl rclcpp std_msgs) +ament_target_dependencies(GripperControl rclcpp std_msgs std_srvs) # Install the plugins install( diff --git a/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh b/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh index da8d416c..97cb2e9f 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh @@ -3,7 +3,9 @@ #include +#include #include +#include #include #include @@ -19,14 +21,12 @@ #include #include #include +#include namespace gripper_control { -class GripperControl : public gz::sim::System, - public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate, - public gz::sim::ISystemPostUpdate +class GripperControl : public gz::sim::System, public gz::sim::ISystemConfigure, public gz::sim::ISystemPreUpdate { public: GripperControl(); @@ -40,14 +40,19 @@ class GripperControl : public gz::sim::System, void PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &_ecm) override; // PostUpdate - called each simulation step (read-only access) - void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override; + // void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override; + + // Service callback + void setOpen(std::shared_ptr const request, + std::shared_ptr response); // Keypress callback void KeypressCallback(std_msgs::msg::String::SharedPtr const msg); private: - // ROS2 Node and subscription + // ROS2 Node, Service, and Subscription rclcpp::Node::SharedPtr node_; + rclcpp::Service::SharedPtr service_; rclcpp::Subscription::SharedPtr key_sub_; // Gazebo transport node + publishers for left and right joints @@ -58,6 +63,8 @@ class GripperControl : public gz::sim::System, // State & settings bool u_pressed_{ false }; bool gripper_open_{ false }; + bool service_called{ false }; + // static std::atomic service_called_flag_; // Atomic double open_pos_{ 0.85 }; // radians (default) double closed_pos_{ 0.0 }; // radians (default) diff --git a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc index 0e52ab10..1784812a 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc @@ -33,6 +33,13 @@ void GripperControl::Configure(gz::sim::Entity const &entity, std::shared_ptrservice_ = this->node_->create_service( + "gripper_control/set_open", + [this](std::shared_ptr const request, + std::shared_ptr response) { this->setOpen(request, response); }); + // Try to obtain model name (from Entity Name component or SDF attribute) auto nameComp = ecm.Component(entity); if (nameComp) @@ -102,6 +109,17 @@ void GripperControl::Configure(gz::sim::Entity const &entity, std::shared_ptr const request, + std::shared_ptr response) +{ + // Set service called flag so PreUpdate moves Gazebo Gripper Model + this->service_called = true; + + // Set response for Servo Client + response->success = true; + response->message = "Gripper open state: " + std::to_string(request->data); +} + void GripperControl::KeypressCallback(std_msgs::msg::String::SharedPtr const msg) { // If message is empty or invalid, ignore @@ -117,10 +135,17 @@ void GripperControl::KeypressCallback(std_msgs::msg::String::SharedPtr const msg } } +// PreUpdate is called so we can use the ecm which is read only in PostUpdate void GripperControl::PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &_ecm) { + if (info.paused) + return; + + // Let ROS2 handle incoming messages for this node + rclcpp::spin_some(this->node_); + // If a 'u' press was detected (set in ROS callback), toggle and apply immediately - if (this->u_pressed_) + if (this->u_pressed_ or this->service_called) { // Set targets; actual motion will be smoothed over subsequent PreUpdate calls this->gripper_open_ = !this->gripper_open_; @@ -128,6 +153,7 @@ void GripperControl::PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityC this->left_target_pos_ = tgt; this->right_target_pos_ = tgt; this->u_pressed_ = false; + this->service_called = false; } // Perform smoothing toward targets each PreUpdate (exponential smoothing) @@ -178,18 +204,7 @@ void GripperControl::PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityC } } -void GripperControl::PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) -{ - if (info.paused) - return; - - // Let ROS2 handle incoming messages for this node - rclcpp::spin_some(this->node_); - (void)ecm; -} - } // namespace gripper_control // Register plugin for Gazebo (include PreUpdate interface so PreUpdate() is called) -GZ_ADD_PLUGIN(gripper_control::GripperControl, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate, - gz::sim::ISystemPostUpdate) +GZ_ADD_PLUGIN(gripper_control::GripperControl, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate) From 62b33f16a18cc9d907c00e3198ab10370889177a Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 23 Mar 2026 16:47:24 -0400 Subject: [PATCH 04/11] Started working on the client side of the Sim Servo Services --- .../package.xml | 7 +++- .../subjugator_sim_actuator_client/setup.py | 14 +++---- .../actuator_client.py | 39 +++++++++++++++++++ 3 files changed, 51 insertions(+), 9 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml b/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml index b2fc3ff3..41bb5bf9 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml @@ -4,7 +4,7 @@ subjugator_sim_actuator_client 0.0.0 TODO: Package description - ckreis + Carlos Chavez TODO: License declaration ament_copyright @@ -12,6 +12,11 @@ ament_pep257 python3-pytest + rclpy + std_msgs + std_srvs + subjugator_msgs + ament_python diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py index b4ac42cd..f3588d0f 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py @@ -12,16 +12,14 @@ ], install_requires=["setuptools"], zip_safe=True, - maintainer="ckreis", - maintainer_email="cckreis0507@gmail.com", + maintainer="Carlos Chavez", + maintainer_email="c.chavez@ufl.edu", description="TODO: Package description", license="TODO: License declaration", - extras_require={ - "test": [ - "pytest", - ], - }, + tests_require=["pytest"], entry_points={ - "console_scripts": [], + "console_scripts": [ + "sim_actuator_client = subjugator_sim_actuator_client.main:main", + ], }, ) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py index e69de29b..82945dbc 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py @@ -0,0 +1,39 @@ +import rclpy +from rclpy.node import Node +from std_srvs.srv import SetBool + + +class SimActuatorClient(Node): + + def __init__(self): + super().__init__("sim_actuator_client") + self._timeout = 1.0 + + # Register GripperControl plugin service client and check if it times out + self.gripper_client = self.create_client(SetBool, "gripper_control/set_open") + while not self.gripper_client.wait_for_service(timeout_sec=self._timeout): + self.get_logger().info("Waiting for gripper_control/set_open service...") + + # Register MarbleDropper plugin service client and check if it times out + self.marble_dropper_client = self.create_client( + SetBool, + "marble_dropper/set_open", + ) + while not self.marble_dropper_client.wait_for_service( + timeout_sec=self._timeout, + ): + self.get_logger().info("Waiting for marble_dropper/set_open service...") + + # Register Torpedo plugin service client and check if it times out + self.torpedo_client = self.create_client(SetBool, "torpedo/fire") + while not self.torpedo_client.wait_for_service(timeout_sec=self._timeout): + self.get_logger().info("Waiting for torpedo/fire service...") + + +def main(): + rclpy.init() + # clientNode = SimActuatorClient() + + +if __name__ == "__main__": + main() From 36a97bb569f720ce71e0bbd78e66320d2547e673 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 30 Mar 2026 15:37:37 -0400 Subject: [PATCH 05/11] Service calls work! Sub9 flies away once opened. Learning how to control server through client (Carlos teach :)) --- .../subjugator_sim_actuator_client/setup.py | 4 +- .../actuator_client.py | 40 +++++++++++++------ .../subjugator_gazebo/src/GripperControl.cc | 1 + 3 files changed, 31 insertions(+), 14 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py index f3588d0f..19d89fce 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py @@ -16,10 +16,10 @@ maintainer_email="c.chavez@ufl.edu", description="TODO: Package description", license="TODO: License declaration", - tests_require=["pytest"], + # tests_require=["pytest"], entry_points={ "console_scripts": [ - "sim_actuator_client = subjugator_sim_actuator_client.main:main", + "sim_actuator_client = subjugator_sim_actuator_client.actuator_client:main", ], }, ) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py index 82945dbc..40a1cbaf 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py @@ -15,24 +15,40 @@ def __init__(self): self.get_logger().info("Waiting for gripper_control/set_open service...") # Register MarbleDropper plugin service client and check if it times out - self.marble_dropper_client = self.create_client( - SetBool, - "marble_dropper/set_open", - ) - while not self.marble_dropper_client.wait_for_service( - timeout_sec=self._timeout, - ): - self.get_logger().info("Waiting for marble_dropper/set_open service...") + # self.marble_dropper_client = self.create_client(SetBool,"marble_dropper/set_open") + # while not self.marble_dropper_client.wait_for_service(timeout_sec=self._timeout): + # self.get_logger().info("Waiting for marble_dropper/set_open service...") # Register Torpedo plugin service client and check if it times out - self.torpedo_client = self.create_client(SetBool, "torpedo/fire") - while not self.torpedo_client.wait_for_service(timeout_sec=self._timeout): - self.get_logger().info("Waiting for torpedo/fire service...") + # self.torpedo_client = self.create_client(SetBool, "torpedo/fire") + # while not self.torpedo_client.wait_for_service(timeout_sec=self._timeout): + # self.get_logger().info("Waiting for torpedo/fire service...") + + # Create request and populate with boolean status, then call the appropriate service client + def actuate_gripper(self, isOpen: bool): + request = SetBool.Request() + request.data = isOpen + self.gripper_client.call_async(request) + + def actuate_marble_dropper(self, isOpen: bool): + request = SetBool.Request() + request.data = isOpen + self.marble_dropper_client.call_async(request) + + def actuate_torpedo(self, launchStatus: bool): + request = SetBool.Request() + request.data = launchStatus + self.torpedo_client.call_async(request) def main(): rclpy.init() - # clientNode = SimActuatorClient() + clientNode = SimActuatorClient() + clientNode.actuate_gripper(True) + # clientNode.actuate_marble_dropper(True) + # clientNode.actuate_torpedo(True) + rclpy.spin(clientNode) + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc index 1784812a..972ec128 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc @@ -118,6 +118,7 @@ void GripperControl::setOpen(std::shared_ptr co // Set response for Servo Client response->success = true; response->message = "Gripper open state: " + std::to_string(request->data); + std::cout << "[GripperControl] Service call received. Setting gripper status: " << request->data << std::endl; } void GripperControl::KeypressCallback(std_msgs::msg::String::SharedPtr const msg) From 9998d69ae463494184525fe5fb8abbc2fc0a621d Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 30 Mar 2026 16:26:07 -0400 Subject: [PATCH 06/11] Client successfully triggers service (thank you dean :]). Now I talk to Carlos for next steps --- .../subjugator_sim_actuator_client/actuator_client.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py index 40a1cbaf..fb99fbcf 100644 --- a/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py @@ -28,7 +28,8 @@ def __init__(self): def actuate_gripper(self, isOpen: bool): request = SetBool.Request() request.data = isOpen - self.gripper_client.call_async(request) + future = self.gripper_client.call_async(request) + rclpy.spin_until_future_complete(self, future, timeout_sec=2.0) def actuate_marble_dropper(self, isOpen: bool): request = SetBool.Request() @@ -44,9 +45,13 @@ def actuate_torpedo(self, launchStatus: bool): def main(): rclpy.init() clientNode = SimActuatorClient() - clientNode.actuate_gripper(True) # clientNode.actuate_marble_dropper(True) # clientNode.actuate_torpedo(True) + # for _ in range(5): + # clientNode.actuate_gripper(True) + # rclpy.spin_once(clientNode, timeout_sec=2.0) + # clientNode.actuate_gripper(False) + # rclpy.spin_once(clientNode, timeout_sec=2.0) rclpy.spin(clientNode) rclpy.shutdown() From ded3acd880afec3dbcc949f772e556bd7747d1ca Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 6 Apr 2026 15:38:38 -0400 Subject: [PATCH 07/11] I some of the basic code to be in the mission planner. I am working in a somewhat closed off environment (I commented a lot of stuff out lol) and I am now working on implementing the functionality into an operation --- src/subjugator/mission_planner/CMakeLists.txt | 6 +- .../mission_planner/include/context.hpp | 30 +++ .../src/mission_planner_node.cpp | 240 ++++++++++-------- .../include/actuate_servo.hpp | 26 ++ .../src/actuate_servo.cpp | 84 ++++++ 5 files changed, 275 insertions(+), 111 deletions(-) create mode 100644 src/subjugator/mission_planner/subjugator_operations/include/actuate_servo.hpp create mode 100644 src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 4e1527cb..da34ceb1 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/src/subjugator/mission_planner/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(mil_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) @@ -32,7 +33,8 @@ add_library( subjugator_operations/src/poles_big_enough.cpp subjugator_operations/src/determine_channel_side.cpp subjugator_operations/src/any_poles_detected.cpp - subjugator_operations/src/has_found_pair.cpp) + subjugator_operations/src/has_found_pair.cpp + subjugator_operations/src/actuate_servo.cpp) ament_target_dependencies( operations @@ -40,6 +42,7 @@ ament_target_dependencies( geometry_msgs nav_msgs mil_msgs + std_srvs sensor_msgs yolo_msgs behaviortree_cpp @@ -56,6 +59,7 @@ ament_target_dependencies( geometry_msgs nav_msgs mil_msgs + std_srvs sensor_msgs yolo_msgs behaviortree_cpp diff --git a/src/subjugator/mission_planner/include/context.hpp b/src/subjugator/mission_planner/include/context.hpp index a7e4884e..0df1e635 100644 --- a/src/subjugator/mission_planner/include/context.hpp +++ b/src/subjugator/mission_planner/include/context.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include struct Context @@ -19,6 +20,11 @@ struct Context rclcpp::Subscription::SharedPtr targets_sub; rclcpp::Subscription::SharedPtr image_sub; + // Service Clients + rclcpp::Client::SharedPtr gripper_client; + rclcpp::Client::SharedPtr marble_client; + rclcpp::Client::SharedPtr torpedo_client; + // Latest state std::mutex odom_mx; std::optional latest_odom; @@ -38,3 +44,27 @@ struct Context return node->get_logger(); } }; + +// Function: Actuate a Service Client to open or close a servo +// Output: Returns a boolean indicating whether the service call was successful (true) or not (false) +bool actuateServo(std::shared_ptr node, rclcpp::Client::SharedPtr client, + bool isOpen) +{ + auto request = std::make_shared(); + request->data = isOpen; + auto future = client->async_send_request(request); + std::string clientName = client->get_service_name(); + + // Spin Node until future completes, or timeout after 2 second + if (rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(2)) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(node->get_logger(), "Servo actuation for %s: %s", clientName.c_str(), + future.get()->success ? "true" : "false"); + return true; + } + else + { + RCLCPP_ERROR(node->get_logger(), "Failed to call %s service", clientName.c_str()); + return false; + } +} diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index 686bd19b..363c2ffd 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -33,118 +34,137 @@ int main(int argc, char** argv) auto ctx = std::make_shared(); ctx->node = node; - node->declare_parameter("mission", "SonarFollowerTest"); - std::string mission_to_run = node->get_parameter("mission").as_string(); - - // Topics to subscribe/publish to - ctx->goal_pub = node->create_publisher("/goal_pose", 10); - ctx->odom_sub = node->create_subscription("/odometry/filtered", 10, - [ctx](nav_msgs::msg::Odometry::SharedPtr msg) - { - std::scoped_lock lk(ctx->odom_mx); - ctx->latest_odom = *msg; - }); - - // Perception targets: from your YOLO node - ctx->targets_sub = - node->create_subscription("/yolo/detections", 10, - [ctx](yolo_msgs::msg::DetectionArray::SharedPtr msg) - { - std::scoped_lock lk(ctx->detections_mx); - ctx->latest_detections = *msg; - }); - - // Image size (for pixel->angle mapping). Probably do not need - ctx->image_sub = node->create_subscription("/front_cam/image_raw", 10, - [ctx](sensor_msgs::msg::Image::SharedPtr msg) - { - std::scoped_lock lk(ctx->img_mx); - ctx->img_width = msg->width; - ctx->img_height = msg->height; - }); - - // Wait for odometry before starting mission - RCLCPP_INFO(node->get_logger(), "Waiting for odometry..."); - rclcpp::Rate wait_rate(10.0); - while (rclcpp::ok()) - { - rclcpp::spin_some(node); - { - std::scoped_lock lk(ctx->odom_mx); - if (ctx->latest_odom.has_value()) - { - RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!"); - break; - } - } - wait_rate.sleep(); - } - - BT::BehaviorTreeFactory factory; - factory.registerNodeType("PublishGoalPose"); - factory.registerNodeType("AtGoalPose"); - factory.registerNodeType("DetectTarget"); - factory.registerNodeType("HoneBearing"); - factory.registerNodeType("CheckYoloModel"); - factory.registerNodeType("TrackLargestPoles"); - factory.registerNodeType("PolesBigEnough"); - factory.registerNodeType("DetermineChannelSide"); - factory.registerNodeType("AnyPolesDetected"); - factory.registerNodeType("HasFoundPair"); - - factory.registerNodeType>("TopicTicker"); - factory.registerNodeType("CountWhenTicked"); - factory.registerNodeType("SonarFollower"); - factory.registerNodeType("YawStyle"); - - // Load all tree models from installed xml - std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner"); - std::string const bt_dir = (std::filesystem::path(pkg_share) / "bt").string(); - auto bt_path = [&](std::string const& file) { return (std::filesystem::path(bt_dir) / file).string(); }; - factory.registerBehaviorTreeFromFile(bt_path("sub9_missions.xml")); - - // Create by name - auto blackboard = BT::Blackboard::create(); - blackboard->set("ctx", ctx); - - std::unique_ptr tree_ptr; - try - { - tree_ptr = std::make_unique(factory.createTree(mission_to_run, blackboard)); - } - catch (std::exception const& e) - { - RCLCPP_FATAL(node->get_logger(), "Unknown mission '%s' Error: %s", mission_to_run.c_str(), e.what()); - rclcpp::shutdown(); - return 1; - } - - // For live feed of tree - BT::Groot2Publisher publisher(*tree_ptr); - - // Log BT transitions to console - BT::StdCoutLogger logger_cout(*tree_ptr); - - RCLCPP_INFO(node->get_logger(), "Mission Planner started. Ticking tree…"); - rclcpp::WallRate rate(20.0); - - std::string xml_models = BT::writeTreeNodesModelXML(factory); - std::ofstream("/home/carlos/models.xml") << xml_models; + // INSERT TEST CODE FOR GRIPPER CONTROL PLUGIN HERE - while (rclcpp::ok()) + ctx->gripper_client = node->create_client("gripper_control/set_open"); + while (!ctx->gripper_client->wait_for_service(std::chrono::seconds(1))) { - rclcpp::spin_some(node); - BT::NodeStatus status = tree_ptr->tickOnce(); - - if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) - { - RCLCPP_INFO(node->get_logger(), "Mission finished with status: %s. Shutting down.", - (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE")); - tree_ptr->haltTree(); - break; - } - rate.sleep(); - } + }; + + // actuateServo(node, ctx->gripper_client, true); + + // for (int i = 0; i < 5; i++){ + // actuateServo(node, ctx->gripper_client, false); + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // actuateServo(node, ctx->gripper_client, true); + // std::this_thread::sleep_for(std::chrono::seconds(1)); + // } + + // END TEST CODE + + // node->declare_parameter("mission", "SonarFollowerTest"); + // std::string mission_to_run = node->get_parameter("mission").as_string(); + + // // Topics to subscribe/publish to + // ctx->goal_pub = node->create_publisher("/goal_pose", 10); + // ctx->odom_sub = node->create_subscription("/odometry/filtered", 10, + // [ctx](nav_msgs::msg::Odometry::SharedPtr msg) + // { + // std::scoped_lock lk(ctx->odom_mx); + // ctx->latest_odom = *msg; + // }); + + // // Perception targets: from your YOLO node + // ctx->targets_sub = + // node->create_subscription("/yolo/detections", 10, + // [ctx](yolo_msgs::msg::DetectionArray::SharedPtr + // msg) + // { + // std::scoped_lock lk(ctx->detections_mx); + // ctx->latest_detections = *msg; + // }); + + // // Image size (for pixel->angle mapping). Probably do not need + // ctx->image_sub = node->create_subscription("/front_cam/image_raw", 10, + // [ctx](sensor_msgs::msg::Image::SharedPtr msg) + // { + // std::scoped_lock lk(ctx->img_mx); + // ctx->img_width = msg->width; + // ctx->img_height = msg->height; + // }); + + // // Wait for odometry before starting mission + // RCLCPP_INFO(node->get_logger(), "Waiting for odometry..."); + // rclcpp::Rate wait_rate(10.0); + // while (rclcpp::ok()) + // { + // rclcpp::spin_some(node); + // { + // std::scoped_lock lk(ctx->odom_mx); + // if (ctx->latest_odom.has_value()) + // { + // RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!"); + // break; + // } + // } + // wait_rate.sleep(); + // } + + // BT::BehaviorTreeFactory factory; + // factory.registerNodeType("PublishGoalPose"); + // factory.registerNodeType("AtGoalPose"); + // factory.registerNodeType("DetectTarget"); + // factory.registerNodeType("HoneBearing"); + // factory.registerNodeType("CheckYoloModel"); + // factory.registerNodeType("TrackLargestPoles"); + // factory.registerNodeType("PolesBigEnough"); + // factory.registerNodeType("DetermineChannelSide"); + // factory.registerNodeType("AnyPolesDetected"); + // factory.registerNodeType("HasFoundPair"); + + // factory.registerNodeType>("TopicTicker"); + // factory.registerNodeType("CountWhenTicked"); + // factory.registerNodeType("SonarFollower"); + // factory.registerNodeType("YawStyle"); + + // // Load all tree models from installed xml + // std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner"); + // std::string const bt_dir = (std::filesystem::path(pkg_share) / "bt").string(); + // auto bt_path = [&](std::string const& file) { return (std::filesystem::path(bt_dir) / file).string(); }; + // factory.registerBehaviorTreeFromFile(bt_path("sub9_missions.xml")); + + // // Create by name + // auto blackboard = BT::Blackboard::create(); + // blackboard->set("ctx", ctx); + + // std::unique_ptr tree_ptr; + // try + // { + // tree_ptr = std::make_unique(factory.createTree(mission_to_run, blackboard)); + // } + // catch (std::exception const& e) + // { + // RCLCPP_FATAL(node->get_logger(), "Unknown mission '%s' Error: %s", mission_to_run.c_str(), e.what()); + // rclcpp::shutdown(); + // return 1; + // } + + // // For live feed of tree + // BT::Groot2Publisher publisher(*tree_ptr); + + // // Log BT transitions to console + // BT::StdCoutLogger logger_cout(*tree_ptr); + + // RCLCPP_INFO(node->get_logger(), "Mission Planner started. Ticking tree…"); + // rclcpp::WallRate rate(20.0); + + // std::string xml_models = BT::writeTreeNodesModelXML(factory); + // std::ofstream("/home/carlos/models.xml") << xml_models; + + // while (rclcpp::ok()) + // { + // rclcpp::spin_some(node); + // BT::NodeStatus status = tree_ptr->tickOnce(); + + // if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) + // { + // RCLCPP_INFO(node->get_logger(), "Mission finished with status: %s. Shutting down.", + // (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE")); + // tree_ptr->haltTree(); + // break; + // } + // rate.sleep(); + // } rclcpp::shutdown(); return 0; diff --git a/src/subjugator/mission_planner/subjugator_operations/include/actuate_servo.hpp b/src/subjugator/mission_planner/subjugator_operations/include/actuate_servo.hpp new file mode 100644 index 00000000..e0f49564 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/include/actuate_servo.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include "context.hpp" +#include "operations.hpp" + +// Passes through request from mission planner to open or close a servo based on input port +// Output port will return status of the servo +class ActuateServo final : public BT::ConditionNode +{ + public: + ActuateServo(std::string const& name, const BT::NodeConfiguration& cfg) : BT::ConditionNode(name, cfg) + { + } + + static BT::PortsList providedPorts(); + BT::NodeStatus tick() override; + + private: + std::shared_ptr ctx_; + int servo_id_; + bool gripper_is_open_; // ID: 1 + bool marble_is_drop_; // ID: 2 + bool torp_is_launch_; // ID: 3 +}; diff --git a/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp b/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp new file mode 100644 index 00000000..86ce5704 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp @@ -0,0 +1,84 @@ +#include "actuate_servo.hpp" + +BT::PortsList ActuateServo::providedPorts() +{ + BT::PortsList ports; + + // Input ports + ports.insert(BT::InputPort>("ctx", "Shared Context")); + ports.insert(BT::InputPort("gripper_is_open", "Whether to open (true) or close (false) the gripper")); + ports.insert(BT::InputPort("marble_is_drop", "Whether the marble is being dropped (true) or not (false)")); + ports.insert(BT::InputPort("torp_is_launch", "Whether the torpedo is being launched (true) or not (false)")); + ports.insert(BT::InputPort("servo_id", "ID of the servo to actuate (e.g. 1 for gripper, 2 for marble drop, 3 " + "for torpedo launcher)")); + // Servo IDs: + // 1. Gripper + // 2. Marble Dropper + // 3. Torpedo Launcher + + // Outputs ports + ports.insert(BT::OutputPort("gripper_status")); // The status of the servo acutation attempt + + return ports; +} + +BT::NodeStatus ActuateServo::tick() +{ + // Get Context - return FAILURE if not available + if (!ctx_ && (!getInput("ctx", ctx_) || !ctx_)) + { + setOutput("status", "Failed to get Context"); + return BT::NodeStatus::FAILURE; + } + + // Get Servo ID - return FAILURE if not available + if (!getInput("servo_id", servo_id_)) + { + setOutput("status", "Failed to get servo_id"); + return BT::NodeStatus::FAILURE; + } + + // Choose what ports to read from based on servo ID + switch (servo_id_) + { + case 1: // Gripper + if (!getInput("gripper_is_open", gripper_is_open_)) + { + setOutput("status", "Failed to get gripper_is_open input"); + return BT::NodeStatus::FAILURE; + } + + break; + case 2: // Marble Dropper + if (!getInput("marble_is_drop", marble_is_drop_)) + { + setOutput("status", "Failed to get marble_is_drop input"); + return BT::NodeStatus::FAILURE; + } + break; + case 3: // Torpedo Launcher + if (!getInput("torp_is_launch", torp_is_launch_)) + { + setOutput("status", "Failed to get torp_is_launch input"); + return BT::NodeStatus::FAILURE; + } + break; + default: + setOutput("status", "Invalid servo_id input"); + return BT::NodeStatus::FAILURE; + } + + // + + // Call actuateServo function and set status output based on result + if (actuateServo(ctx_->node, ctx_->gripper_client, is_open_)) + { + setOutput("status", is_open_ ? true : false); + return BT::NodeStatus::SUCCESS; + } + else + { + setOutput("status", is_open_ ? true : false); + return BT::NodeStatus::FAILURE; + } +} From 4f0eac84c84a2dd68062d322e35a1e4aac528161 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Mon, 6 Apr 2026 15:49:05 -0400 Subject: [PATCH 08/11] I finished writing all the code for the operation and now I need to add a mission.xml and then run it with the mission planner to verify that all my servo actuation stuff is working --- .../src/mission_planner_node.cpp | 1 + .../src/actuate_servo.cpp | 52 +++++++++++++------ 2 files changed, 38 insertions(+), 15 deletions(-) diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index 363c2ffd..63d5294a 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -7,6 +7,7 @@ #include +#include "actuate_servo.hpp" #include "any_poles_detected.hpp" #include "at_goal_pose.hpp" #include "check_yolo_model.hpp" diff --git a/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp b/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp index 86ce5704..3c2008db 100644 --- a/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp +++ b/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp @@ -39,46 +39,68 @@ BT::NodeStatus ActuateServo::tick() } // Choose what ports to read from based on servo ID + // Then actuate corresponding servo and set status output based on success of actuation and desired state switch (servo_id_) { case 1: // Gripper + // Get gripper_is_open input if (!getInput("gripper_is_open", gripper_is_open_)) { setOutput("status", "Failed to get gripper_is_open input"); return BT::NodeStatus::FAILURE; } - + // Actuate gripper and set status output based on gripper_is_open value + if (actuateServo(ctx_->node, ctx_->gripper_client, gripper_is_open_)) + { + setOutput("status", gripper_is_open_ ? true : false); + return BT::NodeStatus::SUCCESS; + } + else + { + setOutput("status", gripper_is_open_ ? true : false); + return BT::NodeStatus::FAILURE; + } break; case 2: // Marble Dropper + // Get marble_is_drop input if (!getInput("marble_is_drop", marble_is_drop_)) { setOutput("status", "Failed to get marble_is_drop input"); return BT::NodeStatus::FAILURE; } + // Actuate marble dropper and set status output based on marble_is_drop value + if (actuateServo(ctx_->node, ctx_->marble_client, marble_is_drop_)) + { + setOutput("status", marble_is_drop_ ? true : false); + return BT::NodeStatus::SUCCESS; + } + else + { + setOutput("status", marble_is_drop_ ? true : false); + return BT::NodeStatus::FAILURE; + } break; case 3: // Torpedo Launcher + // Get torp_is_launch input if (!getInput("torp_is_launch", torp_is_launch_)) { setOutput("status", "Failed to get torp_is_launch input"); return BT::NodeStatus::FAILURE; } + // Actuate torpedo launcher and set status output based on torp_is_launch value + if (actuateServo(ctx_->node, ctx_->torpedo_client, torp_is_launch_)) + { + setOutput("status", torp_is_launch_ ? true : false); + return BT::NodeStatus::SUCCESS; + } + else + { + setOutput("status", torp_is_launch_ ? true : false); + return BT::NodeStatus::FAILURE; + } break; default: setOutput("status", "Invalid servo_id input"); return BT::NodeStatus::FAILURE; } - - // - - // Call actuateServo function and set status output based on result - if (actuateServo(ctx_->node, ctx_->gripper_client, is_open_)) - { - setOutput("status", is_open_ ? true : false); - return BT::NodeStatus::SUCCESS; - } - else - { - setOutput("status", is_open_ ? true : false); - return BT::NodeStatus::FAILURE; - } } From 56f38681486f593b4038dc7118fd8884ade944cc Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Sun, 19 Apr 2026 21:30:50 -0400 Subject: [PATCH 09/11] Created mission called 'test_servos.xml' which used the operation actuate_servos to move the gripper with certain inputs/parameters. The missionwas initialized and used in mission_planner_node too. It seems to have tested well and should be good. Now to add the other plugin functionalities and test with all of them. --- .../mission_planner/include/context.hpp | 4 +- .../src/mission_planner_node.cpp | 95 ++++++++++++++++++- .../subjugator_missions/xml/test_servos.xml | 24 +++++ 3 files changed, 118 insertions(+), 5 deletions(-) create mode 100644 src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml diff --git a/src/subjugator/mission_planner/include/context.hpp b/src/subjugator/mission_planner/include/context.hpp index 0df1e635..3d3e0413 100644 --- a/src/subjugator/mission_planner/include/context.hpp +++ b/src/subjugator/mission_planner/include/context.hpp @@ -47,8 +47,8 @@ struct Context // Function: Actuate a Service Client to open or close a servo // Output: Returns a boolean indicating whether the service call was successful (true) or not (false) -bool actuateServo(std::shared_ptr node, rclcpp::Client::SharedPtr client, - bool isOpen) +inline bool actuateServo(std::shared_ptr node, rclcpp::Client::SharedPtr client, + bool isOpen) { auto request = std::make_shared(); request->data = isOpen; diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index 63d5294a..a69df162 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -51,8 +52,6 @@ int main(int argc, char** argv) // std::this_thread::sleep_for(std::chrono::seconds(1)); // } - // END TEST CODE - // node->declare_parameter("mission", "SonarFollowerTest"); // std::string mission_to_run = node->get_parameter("mission").as_string(); @@ -101,7 +100,97 @@ int main(int argc, char** argv) // wait_rate.sleep(); // } - // BT::BehaviorTreeFactory factory; + BT::BehaviorTreeFactory factory; + factory.registerNodeType("ActuateServo"); + // Load and run mission by name. Default mission is TestServos which + // exercises the `ActuateServo` node for servo IDs 1,2,3 toggling true/false. + node->declare_parameter("mission", "TestServos"); + std::string mission_to_run = node->get_parameter("mission").as_string(); + + std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner"); + std::string const bt_dir = (std::filesystem::path(pkg_share) / "subjugator_missions" / "xml").string(); + auto bt_path = [&](std::string const& file) { return (std::filesystem::path(bt_dir) / file).string(); }; + + std::string mission_file = mission_to_run + ".xml"; + std::string fallback_file = "test_servos.xml"; + + try + { + std::string file_to_load = bt_path(mission_file); + if (!std::filesystem::exists(file_to_load)) + { + file_to_load = bt_path(fallback_file); + } + // If not found in installed package share, try to locate the file + // somewhere in the workspace (useful when running from source). + if (!std::filesystem::exists(file_to_load)) + { + for (auto const& entry : std::filesystem::recursive_directory_iterator(std::filesystem::current_path())) + { + if (!entry.is_regular_file()) + continue; + if (entry.path().filename() == fallback_file) + { + file_to_load = entry.path().string(); + break; + } + } + } + + if (!std::filesystem::exists(file_to_load)) + { + RCLCPP_FATAL(node->get_logger(), "Behavior tree file not found: %s", file_to_load.c_str()); + rclcpp::shutdown(); + return 1; + } + + factory.registerBehaviorTreeFromFile(file_to_load); + } + catch (std::exception const& e) + { + RCLCPP_FATAL(node->get_logger(), "Failed to load behavior tree file: %s", e.what()); + rclcpp::shutdown(); + return 1; + } + + auto blackboard = BT::Blackboard::create(); + blackboard->set("ctx", ctx); + + std::unique_ptr tree_ptr; + try + { + tree_ptr = std::make_unique(factory.createTree(mission_to_run, blackboard)); + } + catch (std::exception const& e) + { + RCLCPP_FATAL(node->get_logger(), "Unknown mission '%s' Error: %s", mission_to_run.c_str(), e.what()); + rclcpp::shutdown(); + return 1; + } + + BT::Groot2Publisher publisher(*tree_ptr); + BT::StdCoutLogger logger_cout(*tree_ptr); + + RCLCPP_INFO(node->get_logger(), "Mission Planner started. Ticking tree…"); + rclcpp::WallRate rate(20.0); + + while (rclcpp::ok()) + { + rclcpp::spin_some(node); + BT::NodeStatus status = tree_ptr->tickOnce(); + + if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) + { + RCLCPP_INFO(node->get_logger(), "Mission finished with status: %s. Shutting down.", + (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE")); + tree_ptr->haltTree(); + break; + } + rate.sleep(); + } + + // END TEST CODE + // factory.registerNodeType("PublishGoalPose"); // factory.registerNodeType("AtGoalPose"); // factory.registerNodeType("DetectTarget"); diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml b/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml new file mode 100644 index 00000000..d08062bb --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + From b28af626c677ac26b67531dc7c3239d1dbb37b52 Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Wed, 22 Apr 2026 14:09:01 -0400 Subject: [PATCH 10/11] Lots of changes ..sorry. I added the Torpedo plugin and model into this branch. I have also added services for the Marble Dropper and Torpedo plugins. I am testing now but everything looks done. Figuring out some kinks rn but should be done very soon --- .../src/mission_planner_node.cpp | 11 + .../subjugator_missions/xml/test_servos.xml | 21 +- .../models/torpedo/torpedo.sdf | 62 +++++ .../urdf/sub9_sim.urdf.xacro | 2 + .../subjugator_gazebo/CMakeLists.txt | 16 +- .../include/GripperControl.hh | 3 - .../include/MarbleDropper.hh | 9 +- .../subjugator_gazebo/include/Torpedoes.hh | 104 ++++++++ .../simulation/subjugator_gazebo/package.xml | 1 + .../subjugator_gazebo/src/MarbleDropper.cc | 26 +- .../subjugator_gazebo/src/Torpedoes.cc | 238 ++++++++++++++++++ 11 files changed, 478 insertions(+), 15 deletions(-) create mode 100644 src/subjugator/simulation/subjugator_description/models/torpedo/torpedo.sdf create mode 100644 src/subjugator/simulation/subjugator_gazebo/include/Torpedoes.hh create mode 100644 src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index a69df162..d24d9527 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -43,6 +43,17 @@ int main(int argc, char** argv) { }; + ctx->marble_client = node->create_client("marble_dropper/drop_marble"); + while (!ctx->marble_client->wait_for_service(std::chrono::seconds(1))) + { + }; + + std::cout << "Creat torp client" << std::endl; + ctx->torpedo_client = node->create_client("torpedo_launcher/launch_torpedo"); + while (!ctx->torpedo_client->wait_for_service(std::chrono::seconds(1))) + { + }; + // actuateServo(node, ctx->gripper_client, true); // for (int i = 0; i < 5; i++){ diff --git a/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml b/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml index d08062bb..16e5e2b3 100644 --- a/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml +++ b/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml @@ -8,17 +8,24 @@ - + + + + + + + + + - - + + + + + - - diff --git a/src/subjugator/simulation/subjugator_description/models/torpedo/torpedo.sdf b/src/subjugator/simulation/subjugator_description/models/torpedo/torpedo.sdf new file mode 100644 index 00000000..10273653 --- /dev/null +++ b/src/subjugator/simulation/subjugator_description/models/torpedo/torpedo.sdf @@ -0,0 +1,62 @@ + + + + false + + + + + .0156 + + 1.18 + -0.003 + 0.04 + 1.431 + -0.034 + 1.262 + + + + + 0.00001 + 0.00001 + + + + + 0 0 0 0 1.57 0 + + + 0.1 + 0.25 + + + + + + + 0 0 0 0 1.57 0 + + + .01 + .125 + + + + 1 0.55 0 1 + + + + + + bodycol + + 50 + + + + 0 0 0 + 0.0000156 + + + diff --git a/src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro b/src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro index 4dec898d..ded27848 100644 --- a/src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro +++ b/src/subjugator/simulation/subjugator_description/urdf/sub9_sim.urdf.xacro @@ -103,6 +103,8 @@ + + gripper_leftArm_joint gripper_rightArm_joint diff --git a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt index 022f693e..8aa891d7 100644 --- a/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt +++ b/src/subjugator/simulation/subjugator_gazebo/CMakeLists.txt @@ -62,6 +62,9 @@ add_library(MarbleDropper SHARED src/MarbleDropper.cc) # Plugin 7: Gripper Controller add_library(GripperControl SHARED src/GripperControl.cc) +# Plugin 8: 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) @@ -79,6 +82,8 @@ target_include_directories(MarbleDropper PRIVATE include) target_include_directories(GripperControl 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 @@ -109,6 +114,10 @@ target_link_libraries(MarbleDropper ament_index_cpp::ament_index_cpp) # Link libraries for GripperControl plugin target_link_libraries(GripperControl 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 @@ -127,10 +136,14 @@ ament_target_dependencies(Hydrophone rclcpp geometry_msgs mil_msgs) ament_target_dependencies(ThrusterBridge rclcpp subjugator_msgs geometry_msgs) -ament_target_dependencies(MarbleDropper rclcpp subjugator_msgs geometry_msgs) +ament_target_dependencies(MarbleDropper rclcpp subjugator_msgs geometry_msgs + std_msgs std_srvs) ament_target_dependencies(GripperControl rclcpp std_msgs std_srvs) +ament_target_dependencies(Torpedoes rclcpp subjugator_msgs geometry_msgs + std_msgs std_srvs) + # Install the plugins install( TARGETS UnderwaterCamera @@ -141,6 +154,7 @@ install( ThrusterBridge MarbleDropper GripperControl + Torpedoes LIBRARY DESTINATION lib/${PROJECT_NAME}) # Install headers diff --git a/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh b/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh index 97cb2e9f..01b841ef 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/GripperControl.hh @@ -39,9 +39,6 @@ class GripperControl : public gz::sim::System, public gz::sim::ISystemConfigure, // PreUpdate - called each simulation step (read-write access) void PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &_ecm) override; - // PostUpdate - called each simulation step (read-only access) - // void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override; - // Service callback void setOpen(std::shared_ptr const request, std::shared_ptr response); diff --git a/src/subjugator/simulation/subjugator_gazebo/include/MarbleDropper.hh b/src/subjugator/simulation/subjugator_gazebo/include/MarbleDropper.hh index 4f7bdc77..0251bd30 100644 --- a/src/subjugator/simulation/subjugator_gazebo/include/MarbleDropper.hh +++ b/src/subjugator/simulation/subjugator_gazebo/include/MarbleDropper.hh @@ -12,6 +12,7 @@ #include #include // For GZ_ADD_PLUGIN +#include // 'gz' and 'sdf' includes turn into namespace::namespace::ClassName #include #include @@ -52,6 +53,10 @@ class MarbleDropper : public gz::sim::System, // System PostUpdate - Called every simulation step to update pinger/marble_dropper distances // void PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager const &ecm) override; + // Service callback + void DropMarble(std::shared_ptr const request, + std::shared_ptr response); + // Callback for keypress void KeypressCallback(std_msgs::msg::String::SharedPtr const msg); @@ -59,8 +64,9 @@ class MarbleDropper : public gz::sim::System, void SpawnMarble(std::string const &worldName, std::string const &sdfPath); private: - // ROS2 Node and Subscription for keypress // + // ROS2 Node, Service, and Subscription for keypress // rclcpp::Node::SharedPtr marble_node_; + rclcpp::Service::SharedPtr service_; rclcpp::Subscription::SharedPtr keypress_sub_; // Sub9 SDF & Entity // @@ -75,6 +81,7 @@ class MarbleDropper : public gz::sim::System, int marbleCount = 0; bool m_pressed = false; // Track if 'm' key was pressed bool worldNameFound = false; + bool service_called = false; // Track if the drop marble service was called std::string worldName = "bingbong"; // Marble SDF Values // diff --git a/src/subjugator/simulation/subjugator_gazebo/include/Torpedoes.hh b/src/subjugator/simulation/subjugator_gazebo/include/Torpedoes.hh new file mode 100644 index 00000000..0e7fa511 --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/include/Torpedoes.hh @@ -0,0 +1,104 @@ +#ifndef TORPEDOES__HH_ +#define TORPEDOES__HH_ + +#include +#include +#include +#include +#include +#include + +#include // For ROS2 + +#include +#include // For GZ_ADD_PLUGIN +#include // For ROS2 service +// 'gz' and 'sdf' includes turn into namespace::namespace::ClassName +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 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; + + // Service Callback + void LaunchTorpedo(std::shared_ptr const request, + std::shared_ptr response); + + // 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, Service, and Subscription for keypress // + rclcpp::Node::SharedPtr torpedo_node_; + rclcpp::Service::SharedPtr service_; + rclcpp::Subscription::SharedPtr keypress_sub_; + + // Sub9 SDF & Entity // + std::shared_ptr 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 torpedoModelNames; // Unique names of torpedo models spawned + std::set torpedoesWithVelocitySet; // Names of torpedoes that have had their velocity set + std::map torpedoSpawnTimes; // Track spawn times of torpedoes + int torpedoCount = 0; + bool t_pressed = false; // Track if 't' key was pressed + bool worldNameFound = false; + bool service_called = false; // Track if the service was called to spawn torpedoes + 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 diff --git a/src/subjugator/simulation/subjugator_gazebo/package.xml b/src/subjugator/simulation/subjugator_gazebo/package.xml index ada8e8b6..1dede655 100644 --- a/src/subjugator/simulation/subjugator_gazebo/package.xml +++ b/src/subjugator/simulation/subjugator_gazebo/package.xml @@ -18,6 +18,7 @@ sdf sensor_msgs std_msgs + std_srvs subjugator_description subjugator_msgs diff --git a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc index 1af2495d..7c735e33 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc @@ -26,6 +26,13 @@ void MarbleDropper::Configure(gz::sim::Entity const &entity, std::shared_ptrservice_ = this->marble_node_->create_service( + "marble_dropper/drop_marble", + [this](std::shared_ptr const request, + std::shared_ptr response) { this->DropMarble(request, response); }); + // Gather necessary information for future GatherWorldInfo() call this->sub9_Entity = entity; @@ -47,6 +54,18 @@ void MarbleDropper::Configure(gz::sim::Entity const &entity, std::shared_ptrworldName << std::endl; } +void MarbleDropper::DropMarble(std::shared_ptr const request, + std::shared_ptr response) +{ + // Set service to request->data to trigger marble drop in PostUpdate depending on requested state + this->service_called = request->data; + + // Set response for Servo Client + response->success = true; + response->message = "Marble dropped state: " + std::to_string(request->data); + std::cout << "[MarbleDropper] Service call received. Marble State: " << request->data << std::endl; +} + void MarbleDropper::KeypressCallback(std_msgs::msg::String::SharedPtr const msg) { // std::cout << "[MarbleDropper] Keypress received: " << msg->data << std::endl; @@ -166,13 +185,14 @@ void MarbleDropper::PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityC } } - // Spawn two marble_dropper maximum, only once per 'm' keypress - if (marbleCount < 2 && m_pressed) + // Spawn two marble_dropper maximum, only once per 'm' keypress or service call + if (marbleCount < 2 && (m_pressed || service_called)) { std::cout << "[MarbleDropper] Spawning marble #" << (marbleCount + 1) << "..." << std::endl; this->SpawnMarble(this->worldName, this->Marble_sdfPath); marbleCount++; - m_pressed = false; // Reset the flag after spawning + m_pressed = false; // Reset the flag after spawning + service_called = false; // Reset the service flag after spawning } } diff --git a/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc b/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc new file mode 100644 index 00000000..e2efc19d --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc @@ -0,0 +1,238 @@ +#include "Torpedoes.hh" + +namespace torpedoes +{ + +Torpedoes::Torpedoes() +{ +} + +Torpedoes::~Torpedoes() +{ +} + +void Torpedoes::Configure(gz::sim::Entity const &entity, std::shared_ptr const &sdf, + gz::sim::EntityComponentManager &ecm, gz::sim::EventManager &eventMgr) +{ + std::cout << "[Torpedoes] Configuring Torpedoes Plugin ..." << std::endl; + + // Initialize the ROS node and keypress subscriber + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + torpedo_node_ = std::make_shared("torpedoes_plugin_node"); + keypress_sub_ = torpedo_node_->create_subscription( + "/keyboard/keypress", 10, std::bind(&torpedoes::Torpedoes::KeypressCallback, this, std::placeholders::_1)); + + // Define ROS2 service for dropping marbles + // Lambda is used so service callback can be a member function with access to class boolean flag + this->service_ = this->torpedo_node_->create_service( + "torpedo_launcher/launch_torpedo", + [this](std::shared_ptr const request, + std::shared_ptr response) { this->LaunchTorpedo(request, response); }); + + // Gather necessary information for future GatherWorldInfo() call + this->sub9_Entity = entity; + + // Create copy of the Sub SDF element to perform modifications due to const + this->sub9_SDF = sdf->Clone(); + + if (!worldNameFound) + { + ecm.Each( + [&](gz::sim::Entity const &entity, gz::sim::components::World const *, + gz::sim::components::Name const *name) -> bool + { + this->worldName = name->Data(); + worldNameFound = true; + return false; + }); + } +} + +void Torpedoes::LaunchTorpedo(std::shared_ptr const request, + std::shared_ptr response) +{ + // Set service to request->data to trigger torpedo launch in PostUpdate depending on requested state + this->service_called = request->data; + + // Set response for Servo Client + response->success = true; + response->message = "Torpedo launch state: " + std::to_string(request->data); + std::cout << "[Torpedoes] Service call received. Torpedo State: " << request->data << std::endl; +} + +void Torpedoes::KeypressCallback(std_msgs::msg::String::SharedPtr const msg) +{ + if (msg->data == "t" && !t_pressed) + { + t_pressed = true; + } +} + +void Torpedoes::PreUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityComponentManager &ecm) +{ + // Only process torpedoes if they have been spawned + if (torpedoCount == 0 || info.paused) + { + return; + } + + // Only process torpedo models that have not had their velocity set + for (auto const &modelName : torpedoModelNames) + { + if (torpedoesWithVelocitySet.count(modelName) > 0) + continue; + // Find the model entity by name + auto modelEntity = ecm.EntityByComponents(gz::sim::components::Name(modelName)); + if (modelEntity == gz::sim::kNullEntity) + { + continue; + } + // Find the 'body' link child entity using ChildrenByComponents + gz::sim::Entity bodyLink = gz::sim::kNullEntity; + auto linkChildren = ecm.ChildrenByComponents(modelEntity, gz::sim::components::Link()); + for (auto linkEntity : linkChildren) + { + auto nameComp = ecm.Component(linkEntity); + if (nameComp && nameComp->Data() == "body") + { + bodyLink = linkEntity; + break; + } + } + if (bodyLink != gz::sim::kNullEntity) + { + gz::sim::Link link(bodyLink); + link.SetLinearVelocity(ecm, gz::math::Vector3d(10.0, 0.0, 0.0)); + // Check if velocity is nonzero, then mark as set + auto velComp = ecm.Component(bodyLink); + if (velComp && velComp->Data().Length() > 0.1) + { + torpedoesWithVelocitySet.insert(modelName); + } + } + } +} + +void Torpedoes::SpawnTorpedo(std::string const &worldName, std::string const &sdfPath) +{ + // Load SDF file as string + std::ifstream sdfFile(sdfPath); + std::stringstream buffer; + buffer << sdfFile.rdbuf(); + std::string sdfString = buffer.str(); + + // Remove XML declaration if present + size_t xmlDeclPos = sdfString.find("", xmlDeclPos); + if (endDecl != std::string::npos) + { + sdfString.erase(xmlDeclPos, endDecl - xmlDeclPos + 2); + } + } + + // Make model name unique for each torpedo (assume double quotes in SDF) + std::string uniqueName = "torpedo_" + std::to_string(torpedoCount + 1); + size_t namePos = sdfString.find("(this->sub9_Entity); + if (sub9_component) + { + this->sub9_pose = sub9_component->Data(); + } + + // Set spawn time for new torpedoes (if not set) + for (auto &pair : torpedoSpawnTimes) + { + if (pair.second < 0.0) + { + pair.second = std::chrono::duration_cast>(info.simTime).count(); + } + } + + // Remove torpedoes after 8 seconds + for (auto spawnedTorpsIter = torpedoSpawnTimes.begin(); spawnedTorpsIter != torpedoSpawnTimes.end();) + { + double timeElapsed = + std::chrono::duration_cast>(info.simTime).count() - spawnedTorpsIter->second; + if (timeElapsed > 8.0) + { + // Remove entity by name + gz::msgs::Entity removeMsg; + removeMsg.set_name(spawnedTorpsIter->first); + removeMsg.set_type(gz::msgs::Entity::MODEL); + std::string removeService = "/world/" + worldName + "/remove"; + node.Request(removeService, removeMsg, timeout, reply, result); + + // Clean up tracking + torpedoModelNames.erase(spawnedTorpsIter->first); + torpedoesWithVelocitySet.erase(spawnedTorpsIter->first); + spawnedTorpsIter = torpedoSpawnTimes.erase(spawnedTorpsIter); + } + else + { + ++spawnedTorpsIter; + } + } + + // Spawn two torpedoes maximum, only once per 't' keypress + if (torpedoCount < 2 && (t_pressed || service_called)) + { + this->SpawnTorpedo(this->worldName, this->Torpedo_sdfPath); + torpedoCount++; + t_pressed = false; // Reset the flag after spawning + } +} + +} // namespace torpedoes + +// Register plugin for Gazebo +GZ_ADD_PLUGIN(torpedoes::Torpedoes, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPostUpdate, + gz::sim::ISystemPreUpdate) From 677509d4a315698ad1a4ac0d3ee23024bbb034ae Mon Sep 17 00:00:00 2001 From: CarterKreis Date: Wed, 22 Apr 2026 14:27:47 -0400 Subject: [PATCH 11/11] My test_servos.xml mission works great with the mission planner. It can individually call each servo using as a service setting true or false to trigger or stop the plugin. --- .../mission_planner/include/context.hpp | 2 +- .../src/mission_planner_node.cpp | 126 +----------------- .../subjugator_gazebo/src/Torpedoes.cc | 8 +- 3 files changed, 8 insertions(+), 128 deletions(-) diff --git a/src/subjugator/mission_planner/include/context.hpp b/src/subjugator/mission_planner/include/context.hpp index 3d3e0413..b490f55e 100644 --- a/src/subjugator/mission_planner/include/context.hpp +++ b/src/subjugator/mission_planner/include/context.hpp @@ -58,7 +58,7 @@ inline bool actuateServo(std::shared_ptr node, rclcpp::Clientget_logger(), "Servo actuation for %s: %s", clientName.c_str(), + RCLCPP_INFO(node->get_logger(), "Servo success status for %s: %s", clientName.c_str(), future.get()->success ? "true" : "false"); return true; } diff --git a/src/subjugator/mission_planner/src/mission_planner_node.cpp b/src/subjugator/mission_planner/src/mission_planner_node.cpp index d24d9527..8f9d52ae 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -54,63 +54,6 @@ int main(int argc, char** argv) { }; - // actuateServo(node, ctx->gripper_client, true); - - // for (int i = 0; i < 5; i++){ - // actuateServo(node, ctx->gripper_client, false); - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // actuateServo(node, ctx->gripper_client, true); - // std::this_thread::sleep_for(std::chrono::seconds(1)); - // } - - // node->declare_parameter("mission", "SonarFollowerTest"); - // std::string mission_to_run = node->get_parameter("mission").as_string(); - - // // Topics to subscribe/publish to - // ctx->goal_pub = node->create_publisher("/goal_pose", 10); - // ctx->odom_sub = node->create_subscription("/odometry/filtered", 10, - // [ctx](nav_msgs::msg::Odometry::SharedPtr msg) - // { - // std::scoped_lock lk(ctx->odom_mx); - // ctx->latest_odom = *msg; - // }); - - // // Perception targets: from your YOLO node - // ctx->targets_sub = - // node->create_subscription("/yolo/detections", 10, - // [ctx](yolo_msgs::msg::DetectionArray::SharedPtr - // msg) - // { - // std::scoped_lock lk(ctx->detections_mx); - // ctx->latest_detections = *msg; - // }); - - // // Image size (for pixel->angle mapping). Probably do not need - // ctx->image_sub = node->create_subscription("/front_cam/image_raw", 10, - // [ctx](sensor_msgs::msg::Image::SharedPtr msg) - // { - // std::scoped_lock lk(ctx->img_mx); - // ctx->img_width = msg->width; - // ctx->img_height = msg->height; - // }); - - // // Wait for odometry before starting mission - // RCLCPP_INFO(node->get_logger(), "Waiting for odometry..."); - // rclcpp::Rate wait_rate(10.0); - // while (rclcpp::ok()) - // { - // rclcpp::spin_some(node); - // { - // std::scoped_lock lk(ctx->odom_mx); - // if (ctx->latest_odom.has_value()) - // { - // RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!"); - // break; - // } - // } - // wait_rate.sleep(); - // } - BT::BehaviorTreeFactory factory; factory.registerNodeType("ActuateServo"); // Load and run mission by name. Default mission is TestServos which @@ -200,73 +143,8 @@ int main(int argc, char** argv) rate.sleep(); } - // END TEST CODE - - // factory.registerNodeType("PublishGoalPose"); - // factory.registerNodeType("AtGoalPose"); - // factory.registerNodeType("DetectTarget"); - // factory.registerNodeType("HoneBearing"); - // factory.registerNodeType("CheckYoloModel"); - // factory.registerNodeType("TrackLargestPoles"); - // factory.registerNodeType("PolesBigEnough"); - // factory.registerNodeType("DetermineChannelSide"); - // factory.registerNodeType("AnyPolesDetected"); - // factory.registerNodeType("HasFoundPair"); - - // factory.registerNodeType>("TopicTicker"); - // factory.registerNodeType("CountWhenTicked"); - // factory.registerNodeType("SonarFollower"); - // factory.registerNodeType("YawStyle"); - - // // Load all tree models from installed xml - // std::string const pkg_share = ament_index_cpp::get_package_share_directory("mission_planner"); - // std::string const bt_dir = (std::filesystem::path(pkg_share) / "bt").string(); - // auto bt_path = [&](std::string const& file) { return (std::filesystem::path(bt_dir) / file).string(); }; - // factory.registerBehaviorTreeFromFile(bt_path("sub9_missions.xml")); - - // // Create by name - // auto blackboard = BT::Blackboard::create(); - // blackboard->set("ctx", ctx); - - // std::unique_ptr tree_ptr; - // try - // { - // tree_ptr = std::make_unique(factory.createTree(mission_to_run, blackboard)); - // } - // catch (std::exception const& e) - // { - // RCLCPP_FATAL(node->get_logger(), "Unknown mission '%s' Error: %s", mission_to_run.c_str(), e.what()); - // rclcpp::shutdown(); - // return 1; - // } - - // // For live feed of tree - // BT::Groot2Publisher publisher(*tree_ptr); - - // // Log BT transitions to console - // BT::StdCoutLogger logger_cout(*tree_ptr); - - // RCLCPP_INFO(node->get_logger(), "Mission Planner started. Ticking tree…"); - // rclcpp::WallRate rate(20.0); - - // std::string xml_models = BT::writeTreeNodesModelXML(factory); - // std::ofstream("/home/carlos/models.xml") << xml_models; - - // while (rclcpp::ok()) - // { - // rclcpp::spin_some(node); - // BT::NodeStatus status = tree_ptr->tickOnce(); - - // if (status == BT::NodeStatus::SUCCESS || status == BT::NodeStatus::FAILURE) - // { - // RCLCPP_INFO(node->get_logger(), "Mission finished with status: %s. Shutting down.", - // (status == BT::NodeStatus::SUCCESS ? "SUCCESS" : "FAILURE")); - // tree_ptr->haltTree(); - // break; - // } - // rate.sleep(); - // } - rclcpp::shutdown(); return 0; + + // END TEST CODE } diff --git a/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc b/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc index e2efc19d..35fd4ddb 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc @@ -56,6 +56,7 @@ void Torpedoes::LaunchTorpedo(std::shared_ptr c { // Set service to request->data to trigger torpedo launch in PostUpdate depending on requested state this->service_called = request->data; + std::cout << "[Torpedoes] Request Data: " << request->data << std::endl; // Set response for Servo Client response->success = true; @@ -157,8 +158,8 @@ void Torpedoes::SpawnTorpedo(std::string const &worldName, std::string const &sd factoryMsg.set_sdf(sdfString); // Set the pose to X, Y, Z, and roll, pitch, yaw - // -0.5 offset in Z to not hit sub9 but will replace once torp launcher model is added - gz::msgs::Set(factoryMsg.mutable_pose(), gz::math::Pose3d(sub9_pose.X(), sub9_pose.Y(), sub9_pose.Z() - 0.5, + // -0.75 offset in Z to not hit sub9 but will REPLACE once torp launcher model is added + gz::msgs::Set(factoryMsg.mutable_pose(), gz::math::Pose3d(sub9_pose.X(), sub9_pose.Y(), sub9_pose.Z() - 0.75, sub9_pose.Roll(), sub9_pose.Pitch(), sub9_pose.Yaw())); // Send the request to create model in .world @@ -227,7 +228,8 @@ void Torpedoes::PostUpdate(gz::sim::UpdateInfo const &info, gz::sim::EntityCompo { this->SpawnTorpedo(this->worldName, this->Torpedo_sdfPath); torpedoCount++; - t_pressed = false; // Reset the flag after spawning + t_pressed = false; // Reset the flag after spawning + service_called = false; // Reset the service call flag } }