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..41bb5bf9 --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/package.xml @@ -0,0 +1,23 @@ + + + + subjugator_sim_actuator_client + 0.0.0 + TODO: Package description + Carlos Chavez + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + std_msgs + std_srvs + subjugator_msgs + + + 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..19d89fce --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/setup.py @@ -0,0 +1,25 @@ +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="Carlos Chavez", + maintainer_email="c.chavez@ufl.edu", + description="TODO: Package description", + license="TODO: License declaration", + # tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "sim_actuator_client = subjugator_sim_actuator_client.actuator_client:main", + ], + }, +) 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..fb99fbcf --- /dev/null +++ b/src/subjugator/gnc/subjugator_sim_actuator_client/subjugator_sim_actuator_client/actuator_client.py @@ -0,0 +1,60 @@ +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...") + + # 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 + 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() + 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.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() + + +if __name__ == "__main__": + main() 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..b490f55e 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) +inline 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 success status 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..8f9d52ae 100644 --- a/src/subjugator/mission_planner/src/mission_planner_node.cpp +++ b/src/subjugator/mission_planner/src/mission_planner_node.cpp @@ -3,10 +3,12 @@ #include #include +#include #include #include +#include "actuate_servo.hpp" #include "any_poles_detected.hpp" #include "at_goal_pose.hpp" #include "check_yolo_model.hpp" @@ -22,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -33,77 +36,77 @@ int main(int argc, char** argv) auto ctx = std::make_shared(); ctx->node = node; - node->declare_parameter("mission", "SonarFollowerTest"); + // INSERT TEST CODE FOR GRIPPER CONTROL PLUGIN HERE + + ctx->gripper_client = node->create_client("gripper_control/set_open"); + while (!ctx->gripper_client->wait_for_service(std::chrono::seconds(1))) + { + }; + + 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))) + { + }; + + 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(); - // 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()) + 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 { - rclcpp::spin_some(node); + 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)) { - std::scoped_lock lk(ctx->odom_mx); - if (ctx->latest_odom.has_value()) + for (auto const& entry : std::filesystem::recursive_directory_iterator(std::filesystem::current_path())) { - RCLCPP_INFO(node->get_logger(), "Odometry received. Starting mission!"); - break; + if (!entry.is_regular_file()) + continue; + if (entry.path().filename() == fallback_file) + { + file_to_load = entry.path().string(); + 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")); + 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; + } - // Create by name auto blackboard = BT::Blackboard::create(); blackboard->set("ctx", ctx); @@ -119,18 +122,12 @@ int main(int argc, char** argv) 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); @@ -148,4 +145,6 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; + + // END TEST CODE } 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..16e5e2b3 --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_missions/xml/test_servos.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + 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..3c2008db --- /dev/null +++ b/src/subjugator/mission_planner/subjugator_operations/src/actuate_servo.cpp @@ -0,0 +1,106 @@ +#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 + // 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; + } +} 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 c7b3bb57..8aa891d7 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) @@ -61,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) @@ -78,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 @@ -108,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 @@ -126,10 +136,13 @@ 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) -# ROS2 / ament dependencies used by your plugin -ament_target_dependencies(GripperControl rclcpp std_msgs) +ament_target_dependencies(Torpedoes rclcpp subjugator_msgs geometry_msgs + std_msgs std_srvs) # Install the plugins install( @@ -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 da8d416c..01b841ef 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(); @@ -39,15 +39,17 @@ class GripperControl : public gz::sim::System, // 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); // 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 +60,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/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/GripperControl.cc b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc index 0809152d..972ec128 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/GripperControl.cc @@ -31,9 +31,15 @@ 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; + // Define ROS2 service for setting gripper open/closed + // Lambda is used so service callback can be a member function with access to class boolean flag + this->service_ = 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) @@ -103,6 +109,18 @@ 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); + std::cout << "[GripperControl] Service call received. Setting gripper status: " << request->data << std::endl; +} + void GripperControl::KeypressCallback(std_msgs::msg::String::SharedPtr const msg) { // If message is empty or invalid, ignore @@ -118,10 +136,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_; @@ -129,6 +154,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) @@ -179,18 +205,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) diff --git a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc index a4af804e..7c735e33 100644 --- a/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc +++ b/src/subjugator/simulation/subjugator_gazebo/src/MarbleDropper.cc @@ -26,12 +26,20 @@ 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; // Create copy of the Sub SDF element to perform modifications due to const this->sub9_SDF = sdf->Clone(); + // Get the world name from the EntityComponentManager (only done once) if (!worldNameFound) { ecm.Each( @@ -46,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; @@ -165,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..35fd4ddb --- /dev/null +++ b/src/subjugator/simulation/subjugator_gazebo/src/Torpedoes.cc @@ -0,0 +1,240 @@ +#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; + std::cout << "[Torpedoes] Request Data: " << request->data << std::endl; + + // 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 + service_called = false; // Reset the service call flag + } +} + +} // namespace torpedoes + +// Register plugin for Gazebo +GZ_ADD_PLUGIN(torpedoes::Torpedoes, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPostUpdate, + gz::sim::ISystemPreUpdate)