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