Carter ros gazebo comm

src/subjugator/gnc/subjugator_sim_actuator_client/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>subjugator_sim_actuator_client</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="c.chavez@ufl.edu">Carlos Chavez</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>subjugator_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

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",
+        ],
+    },
+)

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()

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,14 +33,16 @@ 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
   rclcpp
   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

src/subjugator/mission_planner/include/context.hpp

@@ -7,6 +7,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <yolo_msgs/msg/detection_array.hpp>
 
 struct Context
@@ -19,6 +20,11 @@ struct Context
     rclcpp::Subscription<yolo_msgs::msg::DetectionArray>::SharedPtr targets_sub;
     rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub;
 
+    // Service Clients
+    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr gripper_client;
+    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr marble_client;
+    rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr torpedo_client;
+
     // Latest state
     std::mutex odom_mx;
     std::optional<nav_msgs::msg::Odometry> 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<rclcpp::Node> node, rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr client,
+                         bool isOpen)
+{
+    auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
+    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;
+    }
+}

src/subjugator/mission_planner/src/mission_planner_node.cpp

@@ -3,10 +3,12 @@
 #include <behaviortree_cpp/loggers/groot2_publisher.h>
 #include <behaviortree_cpp/xml_parsing.h>
 
+#include <filesystem>
 #include <fstream>
 
 #include <rclcpp/rclcpp.hpp>
 
+#include "actuate_servo.hpp"
 #include "any_poles_detected.hpp"
 #include "at_goal_pose.hpp"
 #include "check_yolo_model.hpp"
@@ -22,6 +24,7 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <count_when_ticked.hpp>
 #include <go_to_pinger.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <topic_ticker.hpp>
 #include <yaw_style.hpp>
 #include <yolo_msgs/msg/detection_array.hpp>
@@ -33,77 +36,77 @@ int main(int argc, char** argv)
     auto ctx = std::make_shared<Context>();
     ctx->node = node;
 
-    node->declare_parameter<std::string>("mission", "SonarFollowerTest");
+    // INSERT TEST CODE FOR GRIPPER CONTROL PLUGIN HERE
+
+    ctx->gripper_client = node->create_client<std_srvs::srv::SetBool>("gripper_control/set_open");
+    while (!ctx->gripper_client->wait_for_service(std::chrono::seconds(1)))
+    {
+    };
+
+    ctx->marble_client = node->create_client<std_srvs::srv::SetBool>("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<std_srvs::srv::SetBool>("torpedo_launcher/launch_torpedo");
+    while (!ctx->torpedo_client->wait_for_service(std::chrono::seconds(1)))
+    {
+    };
+
+    BT::BehaviorTreeFactory factory;
+    factory.registerNodeType<ActuateServo>("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<std::string>("mission", "TestServos");
     std::string mission_to_run = node->get_parameter("mission").as_string();
 
-    // Topics to subscribe/publish to
-    ctx->goal_pub = node->create_publisher<geometry_msgs::msg::Pose>("/goal_pose", 10);
-    ctx->odom_sub = node->create_subscription<nav_msgs::msg::Odometry>("/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_msgs::msg::DetectionArray>("/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<sensor_msgs::msg::Image>("/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>("PublishGoalPose");
-    factory.registerNodeType<AtGoalPose>("AtGoalPose");
-    factory.registerNodeType<DetectTarget>("DetectTarget");
-    factory.registerNodeType<HoneBearing>("HoneBearing");
-    factory.registerNodeType<CheckYoloModel>("CheckYoloModel");
-    factory.registerNodeType<TrackLargestPoles>("TrackLargestPoles");
-    factory.registerNodeType<PolesBigEnough>("PolesBigEnough");
-    factory.registerNodeType<DetermineChannelSide>("DetermineChannelSide");
-    factory.registerNodeType<AnyPolesDetected>("AnyPolesDetected");
-    factory.registerNodeType<HasFoundPair>("HasFoundPair");
-
-    factory.registerNodeType<TopicTicker<nav_msgs::msg::Odometry>>("TopicTicker");
-    factory.registerNodeType<CountWhenTicked>("CountWhenTicked");
-    factory.registerNodeType<SonarFollower>("SonarFollower");
-    factory.registerNodeType<YawStyle>("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
 }