From 2800e24bb352b4fcd11b847e4366cc5427978c8c Mon Sep 17 00:00:00 2001 From: icolwell-as <48106342+icolwell-as@users.noreply.github.com> Date: Wed, 24 Nov 2021 09:40:46 -0500 Subject: [PATCH] Trigger over Ethernet (#39) * Copy useful pieces from Wilhelm-Marais' branch * Add Action1 in addition to Action0 * Working trigger example, sends on all interfaces for simplicity * Add timer or subscriber-based trigger * Fix some code formatting * Revert sigint handler * Check return value of vimba startup --- CMakeLists.txt | 7 + cfg/AvtVimbaCamera.cfg | 11 +- include/avt_vimba_camera/avt_vimba_camera.h | 4 +- include/avt_vimba_camera/trigger.h | 53 ++++++++ launch/trigger_node.launch | 17 +++ src/avt_vimba_camera.cpp | 24 +++- src/nodes/trigger_node.cpp | 12 ++ src/trigger.cpp | 137 ++++++++++++++++++++ 8 files changed, 261 insertions(+), 4 deletions(-) create mode 100644 include/avt_vimba_camera/trigger.h create mode 100644 launch/trigger_node.launch create mode 100644 src/nodes/trigger_node.cpp create mode 100644 src/trigger.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b028bcd..3a43441d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,6 +95,13 @@ add_executable(sync_node add_dependencies_and_linkings(sync_node) +add_executable(trigger_node + src/nodes/trigger_node.cpp + src/trigger.cpp +) + +add_dependencies_and_linkings(trigger_node) + add_library(avt_camera_nodelets src/nodes/mono_camera_nodelet.cpp src/mono_camera.cpp diff --git a/cfg/AvtVimbaCamera.cfg b/cfg/AvtVimbaCamera.cfg index b834630e..927be89d 100755 --- a/cfg/AvtVimbaCamera.cfg +++ b/cfg/AvtVimbaCamera.cfg @@ -15,7 +15,9 @@ trigger_source_enum = gen.enum( [ gen.const("Freerun", str_t, "Freerun", "Ru gen.const("Line3", str_t, "Line3", "External trigger on SyncIn3 line"), gen.const("Line4", str_t, "Line4", "External trigger on SyncIn4 line"), gen.const("FixedRate", str_t, "FixedRate", "Camera self-triggers at a fixed frame rate defined by `~AcquisitionFrameRateAbs`"), - gen.const("Software", str_t, "Software", "Software inititated image capture")], "Set Trigger Mode") + gen.const("Software", str_t, "Software", "Software inititated image capture"), + gen.const("Action0" , str_t, "Action0", "Trigger over ethernet action0"), + gen.const("Action1" , str_t, "Action1", "Trigger over ethernet action1")], "Set Trigger Mode") trigger_activation_enum = gen.enum([ gen.const("RisingEdge", str_t, "RisingEdge", ""), gen.const("FallingEdge", str_t, "FallingEdge", ""), gen.const("AnyEdge", str_t, "AnyEdge", ""), @@ -27,6 +29,10 @@ trigger_selector_enum = gen.enum([ gen.const("FrameStart", str_t, "FrameS gen.const("AcquisitionStart", str_t, "AcquisitionStart", ""), gen.const("AcquisitionEnd", str_t, "AcquisitionEnd", ""), gen.const("AcquisitionRecord", str_t, "AcquisitionRecord", "")], "Trigger activation selector") +action_selector_enum = gen.enum([ gen.const("ActionDeviceKey", str_t, "ActionDeviceKey", ""), + gen.const("ActionGroupKey", str_t, "ActionGroupKey", ""), + gen.const("ActionGroupMask", str_t, "ActionGroupMask", "")], "Action selection") + acquisition_mode_enum = gen.enum( [ gen.const("Continuous", str_t, "Continuous", "After an acquisition start event, the camera will continuously receive frame trigger events."), gen.const("SingleFrame", str_t, "SingleFrame", "The camera will only deliver a single frame trigger event"), gen.const("MultiFrame", str_t, "MultiFrame", "The camera will acquire the number of images specified by `~AcquisitionFrameCount`. Further trigger events will be ignored"), @@ -110,6 +116,9 @@ gen.add("trigger_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Camer gen.add("trigger_selector", str_t, SensorLevels.RECONFIGURE_STOP, "Camera trigger selector", "FrameStart", edit_method = trigger_selector_enum) gen.add("trigger_activation", str_t, SensorLevels.RECONFIGURE_STOP, "Camera trigger activation", "RisingEdge", edit_method = trigger_activation_enum) gen.add("trigger_delay", double_t, SensorLevels.RECONFIGURE_RUNNING, "Trigger delay in us (only valid when set to external trigger)", 0.0, 0.0, 60000000.0) +gen.add("action_device_key", int_t, SensorLevels.RECONFIGURE_STOP, "Camera action device key", 1, 1, 4294967295) +gen.add("action_group_key", int_t, SensorLevels.RECONFIGURE_STOP, "Camera action group key", 1, 1, 4294967295) +gen.add("action_group_mask", int_t, SensorLevels.RECONFIGURE_STOP, "Camera action group mask", 1, 1, 4294967295) # EXPOSURE gen.add("exposure", double_t, SensorLevels.RECONFIGURE_RUNNING, "Camera exposure time in microseconds.", 50000, 41, 60000000) gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the camera exposure. If continously automatic, causes the `~exposure` setting to be ignored.", "Continuous", edit_method = auto_enum) diff --git a/include/avt_vimba_camera/avt_vimba_camera.h b/include/avt_vimba_camera/avt_vimba_camera.h index 5e4ceb95..1a51511d 100644 --- a/include/avt_vimba_camera/avt_vimba_camera.h +++ b/include/avt_vimba_camera/avt_vimba_camera.h @@ -61,7 +61,9 @@ enum FrameStartTriggerMode { SyncIn1, SyncIn2, SyncIn3, - SyncIn4 + SyncIn4, + Action0, + Action1 }; enum CameraState { diff --git a/include/avt_vimba_camera/trigger.h b/include/avt_vimba_camera/trigger.h new file mode 100644 index 00000000..9e8698db --- /dev/null +++ b/include/avt_vimba_camera/trigger.h @@ -0,0 +1,53 @@ +#ifndef AVT_VIMBA_CAMERA_TRIGGER_H +#define AVT_VIMBA_CAMERA_TRIGGER_H + +#include "VimbaCPP/Include/VimbaCPP.h" + +#include + +#include + +#include + +namespace trigger +{ + +class Trigger +{ +public: + Trigger(); + ~Trigger(); + + void Init(); + +private: + void LoadParams(); + void InitializeAddress(); + bool PrepareActionCommand(); + bool SetIntFeatureValue(const std::string& name, int64_t value); + + void TimerCb(const ros::TimerEvent& event); + void TriggerCb(const std_msgs::Bool::ConstPtr& msg); + void SendActionCommand(); + + AVT::VmbAPI::VimbaSystem& vimba_system_; + AVT::VmbAPI::InterfacePtr interface_ptr_; + + ros::NodeHandle pnh_; + ros::NodeHandle nh_; + + ros::Timer trigger_timer_; + ros::Subscriber trigger_sub_; + + // Params + struct in_addr destination_ip_; + std::string trigger_src_; + float timer_period_; + int action_device_key_; + int action_group_key_; + int action_group_mask_; +}; + +} // namespace trigger + +#endif // AVT_VIMBA_CAMERA_TRIGGER_H diff --git a/launch/trigger_node.launch b/launch/trigger_node.launch new file mode 100644 index 00000000..18c74e6f --- /dev/null +++ b/launch/trigger_node.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/avt_vimba_camera.cpp b/src/avt_vimba_camera.cpp index 0484a298..3793ac9c 100644 --- a/src/avt_vimba_camera.cpp +++ b/src/avt_vimba_camera.cpp @@ -51,7 +51,9 @@ static const char* TriggerMode[] = { "Line1", "Line2", "Line3", - "Line4" }; + "Line4", + "Action0", + "Action1"}; static const char* AcquisitionMode[] = { "Continuous", "SingleFrame", @@ -190,7 +192,9 @@ void AvtVimbaCamera::start(std::string ip_str, std::string guid_str, bool debug_ if (trigger_source_int == Freerun || trigger_source_int == FixedRate || - trigger_source_int == SyncIn1) { + trigger_source_int == SyncIn1 || + trigger_source_int == Action0 || + trigger_source_int == Action1) { // Create a frame observer for this camera SP_SET(frame_obs_ptr_, new FrameObserver(vimba_camera_ptr_, boost::bind(&avt_vimba_camera::AvtVimbaCamera::frameCallback, this, _1))); @@ -662,6 +666,10 @@ int AvtVimbaCamera::getTriggerModeInt(std::string mode_str) { mode = SyncIn3; } else if (mode_str == TriggerMode[SyncIn4]) { mode = SyncIn4; + } else if (mode_str == TriggerMode[Action0]) { + mode = Action0; + } else if (mode_str == TriggerMode[Action1]) { + mode = Action1; } return mode; } @@ -845,6 +853,18 @@ void AvtVimbaCamera::updateAcquisitionConfig(Config& config) { changed = true; setFeatureValue("TriggerDelayAbs", config.trigger_delay); } + if (config.action_device_key != config_.action_device_key || on_init_) { + changed = true; + setFeatureValue("ActionDeviceKey", config.action_device_key); + } + if (config.action_group_key != config_.action_group_key || on_init_) { + changed = true; + setFeatureValue("ActionGroupKey", config.action_group_key); + } + if (config.action_group_mask != config_.action_group_mask || on_init_) { + changed = true; + setFeatureValue("ActionGroupMask", config.action_group_mask); + } if(changed && show_debug_prints_){ ROS_INFO_STREAM("New Acquisition and Trigger config (" << config.frame_id << ") : " << "\n\tAcquisitionMode : " << config.acquisition_mode << " was " << config_.acquisition_mode diff --git a/src/nodes/trigger_node.cpp b/src/nodes/trigger_node.cpp new file mode 100644 index 00000000..d1529e06 --- /dev/null +++ b/src/nodes/trigger_node.cpp @@ -0,0 +1,12 @@ +#include "avt_vimba_camera/trigger.h" + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "trigger_node"); + + trigger::Trigger trigger; + trigger.Init(); + + ros::spin(); + return 0; +} diff --git a/src/trigger.cpp b/src/trigger.cpp new file mode 100644 index 00000000..70f7c3f5 --- /dev/null +++ b/src/trigger.cpp @@ -0,0 +1,137 @@ +#include "avt_vimba_camera/trigger.h" + +namespace trigger +{ + +Trigger::Trigger() : vimba_system_(AVT::VmbAPI::VimbaSystem::GetInstance()), pnh_("~") +{} + +Trigger::~Trigger() +{ + vimba_system_.Shutdown(); +} + +void Trigger::Init() +{ + VmbErrorType return_value = vimba_system_.Startup(); + + if (return_value != VmbErrorSuccess) + { + ROS_ERROR_STREAM("Failed to start Vimba system, vimba error code: " << return_value); + ros::shutdown(); + } + + LoadParams(); + InitializeAddress(); + + if (trigger_src_ == "timer") + { + trigger_timer_ = nh_.createTimer(ros::Duration(timer_period_), &Trigger::TimerCb, this); + } + else if (trigger_src_ == "subscriber") + { + trigger_sub_ = nh_.subscribe("trigger_input", 10, &Trigger::TriggerCb, this); + } + else + { + ROS_ERROR("Unknown trigger_src %s", trigger_src_.c_str()); + ros::shutdown(); + } +} + +void Trigger::LoadParams() +{ + std::string destination_ip; + pnh_.param("destination_ip", destination_ip, "192.168.3.40"); + pnh_.param("trigger_src", trigger_src_, "timer"); + pnh_.param("timer_period", timer_period_, 0.1); + pnh_.param("action_device_key", action_device_key_, 1); + pnh_.param("action_group_key", action_group_key_, 1); + pnh_.param("action_group_mask", action_group_mask_, 1); + + if(inet_aton(destination_ip.c_str(), &destination_ip_) == 0) + { + ROS_ERROR("Unable to parse desination_ip: %s", destination_ip.c_str()); + ros::shutdown(); + } +} + +void Trigger::InitializeAddress() +{ + VmbErrorType return_value = VmbErrorSuccess; + + if (!SetIntFeatureValue("GevActionDestinationIPAddress", destination_ip_.s_addr)) + { + ROS_ERROR("Could not set destination address"); + } + + ROS_INFO("Destination address set"); +} + +bool Trigger::PrepareActionCommand() +{ + return (SetIntFeatureValue("ActionDeviceKey", 1) && + SetIntFeatureValue("ActionGroupKey", 1) && + SetIntFeatureValue("ActionGroupMask", 1)); +} + +// Sets an integer feature value on the vimba system +bool Trigger::SetIntFeatureValue(const std::string& name, int64_t value) +{ + VmbErrorType return_value = VmbErrorSuccess; + + AVT::VmbAPI::FeaturePtr feature_ptr; + return_value = vimba_system_.GetFeatureByName(name.c_str(), feature_ptr); + + if (return_value != VmbErrorSuccess) + { + ROS_ERROR_STREAM("Failed to get feature, vimba error code: " << return_value); + return false; + } + else + { + return_value = feature_ptr->SetValue((VmbInt64_t)value); + } + + return (return_value == VmbErrorSuccess); +} + +void Trigger::TimerCb(const ros::TimerEvent& event) +{ + SendActionCommand(); +} + +void Trigger::TriggerCb(const std_msgs::Bool::ConstPtr& msg) +{ + SendActionCommand(); +} + +void Trigger::SendActionCommand() +{ + if (!PrepareActionCommand()) + { + ROS_ERROR_THROTTLE(1.0, "Failed to prepare action command"); + return; + } + + VmbErrorType return_value = VmbErrorSuccess; + + AVT::VmbAPI::FeaturePtr lFeature; + return_value = vimba_system_.GetFeatureByName("ActionCommand", lFeature ); + + if(return_value == VmbErrorSuccess) + { + return_value = lFeature->RunCommand(); + } + + if (return_value == VmbErrorSuccess) + { + ROS_DEBUG("Action command sent"); + } + else + { + ROS_ERROR_THROTTLE(1.0, "Failed to send action command"); + } +} + +} // namespace trigger