Trigger over Ethernet

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

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)

include/avt_vimba_camera/avt_vimba_camera.h

@@ -61,7 +61,9 @@ enum FrameStartTriggerMode {
   SyncIn1,
   SyncIn2,
   SyncIn3,
-  SyncIn4
+  SyncIn4,
+  Action0,
+  Action1
 };
 
 enum CameraState {

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 <arpa/inet.h>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Bool.h>
+
+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

launch/trigger_node.launch

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="destination_ip" default="192.168.255.255" doc="The ip address of the camera/s to be triggered" />
+  <arg name="trigger_src" default="timer" doc="When to send action commands. Use 'timer' or 'subscriber'" />
+  <arg name="timer_period" default="0.1" doc="Period of timer for sending action commands (s)" />
+  <arg name="action_device_key" default="1" doc="The device key for the sent action" />
+  <arg name="action_group_key" default="1" doc="The group key for the sent actions" />
+  <arg name="action_group_mask" default="1" doc="The group mask for the sent actions" />
+
+  <node name="trigger_node" pkg="avt_vimba_camera" type="trigger_node" output="screen">
+    <param name="destination_ip" value="$(arg destination_ip)" />
+    <param name="trigger_src" value="$(arg trigger_src)" />
+    <param name="timer_period" value="$(arg timer_period)" />
+    <param name="action_device_key" value="$(arg action_device_key)" />
+    <param name="action_group_key" value="$(arg action_group_key)" />
+    <param name="action_group_mask" value="$(arg action_group_mask)" />
+  </node>
+</launch>

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

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;
+}

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<std::string>("destination_ip", destination_ip, "192.168.3.40");
+  pnh_.param<std::string>("trigger_src", trigger_src_, "timer");
+  pnh_.param<float>("timer_period", timer_period_, 0.1);
+  pnh_.param<int>("action_device_key", action_device_key_, 1);
+  pnh_.param<int>("action_group_key", action_group_key_, 1);
+  pnh_.param<int>("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