diff --git a/zebROS_ws/src/controllers_2025/CMakeLists.txt b/zebROS_ws/src/controllers_2025/CMakeLists.txt
index 9773c199c..37bc89cc4 100644
--- a/zebROS_ws/src/controllers_2025/CMakeLists.txt
+++ b/zebROS_ws/src/controllers_2025/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
ctre_interfaces
ddynamic_reconfigure
hardware_interface
+ periodic_interval_counter
pluginlib
realtime_tools
roscpp
@@ -128,6 +129,7 @@ include_directories(
)
add_library(controllers_2025
+ src/camera_trigger_controller.cpp
src/elevator_controller.cpp
)
diff --git a/zebROS_ws/src/controllers_2025/controllers_2025_plugins.xml b/zebROS_ws/src/controllers_2025/controllers_2025_plugins.xml
index b620d8a05..2fab4547e 100644
--- a/zebROS_ws/src/controllers_2025/controllers_2025_plugins.xml
+++ b/zebROS_ws/src/controllers_2025/controllers_2025_plugins.xml
@@ -4,4 +4,9 @@
Library file. Does stuff to make Phoneix6 elevator_controller exist.
+
+
+ Library file. Does stuff to make Phoneix6 camera_trigger_controller exist.
+
+
diff --git a/zebROS_ws/src/controllers_2025/package.xml b/zebROS_ws/src/controllers_2025/package.xml
index daba66de3..ff8ae856b 100644
--- a/zebROS_ws/src/controllers_2025/package.xml
+++ b/zebROS_ws/src/controllers_2025/package.xml
@@ -60,6 +60,7 @@
talon_controllers
ctre_interfaces
ddynamic_reconfigure
+ periodic_interval_counter
controller_interface
controllers_2025_msgs
dynamic_reconfigure_wrapper
@@ -80,7 +81,6 @@
std_msgs
talon_controllers
ctre_interfaces
-
diff --git a/zebROS_ws/src/controllers_2025/src/camera_trigger_controller.cpp b/zebROS_ws/src/controllers_2025/src/camera_trigger_controller.cpp
new file mode 100644
index 000000000..50025547a
--- /dev/null
+++ b/zebROS_ws/src/controllers_2025/src/camera_trigger_controller.cpp
@@ -0,0 +1,79 @@
+#include
+#include
+#include //to compile as a controller
+#include "periodic_interval_counter/periodic_interval_counter.h"
+
+#include "hardware_interface/joint_command_interface.h"
+
+namespace camera_trigger_controller_2025
+{
+
+//this is the actual controller, so it stores all of the update() functions and the actual handle from the joint interface
+class CameraTriggerController_2025 : public controller_interface::Controller {
+public:
+ bool init(hardware_interface::JointCommandInterface *joint_command_interface,
+ ros::NodeHandle & /*root_nh*/,
+ ros::NodeHandle &controller_nh) override
+ {
+ ROS_INFO_STREAM("2025_camera_trigger_controller: init");
+
+ //get publish rate from config file
+ double publish_rate{60.};
+ if (!controller_nh.param("publish_rate", publish_rate, publish_rate))
+ {
+ ROS_WARN("Could not read publish_rate in 2025_camera_trigger_controller");
+ }
+ else if (publish_rate <= 0.0)
+ {
+ ROS_ERROR_STREAM("Invalid publish_rate in 2025_camera_trigger_controller (" << publish_rate << ")");
+ return false;
+ }
+ interval_counter_ = std::make_unique(publish_rate);
+
+ joint_ = joint_command_interface->getHandle("camera_trigger");
+ ROS_INFO_STREAM("2025_camera_trigger_controller: init successful");
+ return true;
+ }
+
+ void starting(const ros::Time &time) override
+ {
+ ROS_INFO_STREAM("2025_camera_trigger_controller: starting");
+ interval_counter_->reset();
+ }
+
+ void update(const ros::Time &time, const ros::Duration &duration) override
+ {
+ // Always add the time increment to the accumulated time.
+ // Keep track of whether the desired period has been reached.
+ timer_expired_ |= interval_counter_->update(duration);
+
+ // Spec says trigger pulse can't be < 2uSec.
+ // Controller is running at 250Hz, or 4mSec per update.
+ // So it should be fine to keep the pulse high for 1 update.
+ if (joint_.getCommand() > 0.0)
+ {
+ joint_.setCommand(0.0);
+ }
+ // If the timer has expired, set the command to 1.0 to
+ // trigger a high pulse for the camera trigger.
+ else if (timer_expired_)
+ {
+ joint_.setCommand(1.0);
+ timer_expired_ = false;
+ }
+ }
+
+ void stopping(const ros::Time & /*time*/) override
+ {
+ }
+
+private:
+ std::unique_ptr interval_counter_;
+ hardware_interface::JointHandle joint_;
+
+ bool timer_expired_{false};
+}; // class
+
+}//namespace
+
+PLUGINLIB_EXPORT_CLASS(camera_trigger_controller_2025::CameraTriggerController_2025, controller_interface::ControllerBase)
\ No newline at end of file