update with driver status and alert

CMakeLists.txt

@@ -3,6 +3,7 @@ project(avt_vimba_camera)
 
 find_package(catkin REQUIRED COMPONENTS
   #libvimba
+  cav_msgs
   camera_info_manager
   diagnostic_updater
   dynamic_reconfigure
@@ -28,8 +29,9 @@ generate_dynamic_reconfigure_options(
 )
 
 catkin_package(
+  #LIBRARIES ${PROJECT_NAME}
   INCLUDE_DIRS include
-  CATKIN_DEPENDS camera_info_manager diagnostic_updater dynamic_reconfigure image_geometry image_transport roscpp sensor_msgs std_msgs polled_camera
+  CATKIN_DEPENDS camera_info_manager diagnostic_updater dynamic_reconfigure image_geometry image_transport roscpp sensor_msgs std_msgs polled_camera cav_msgs
 )
 
 ###########
@@ -58,6 +60,7 @@ function(add_dependencies_and_linkings arg)
   add_dependencies(${arg}
     ${PROJECT_NAME}_gencfg
     #${libvimba_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
   )
 
   if("${ARCH}" STREQUAL i386)
@@ -94,7 +97,8 @@ add_executable(mono_camera_node
   src/mono_camera.cpp
   src/avt_vimba_camera.cpp
   src/frame_observer.cpp
-)
+  src/status_camera.cpp
+    )
 
 add_dependencies_and_linkings(mono_camera_node)
 
@@ -103,23 +107,29 @@ add_executable(stereo_camera_node
   src/stereo_camera.cpp
   src/avt_vimba_camera.cpp
   src/frame_observer.cpp
-)
+  )
 
 add_dependencies_and_linkings(stereo_camera_node)
 
 add_executable(sync_node
   src/nodes/sync_node.cpp
   src/sync.cpp
-)
+  )
 
 add_dependencies_and_linkings(sync_node)
 
+add_library(avt_vimba_camera_lib
+  src/status_camera.cpp
+)
+add_dependencies(avt_vimba_camera_lib ${catkin_EXPORTED_TARGETS})
+
 add_library(avt_camera_nodelets
   src/nodes/mono_camera_nodelet.cpp
   src/nodes/stereo_camera_nodelet.cpp
   src/stereo_camera.cpp
   src/avt_vimba_camera.cpp
   src/frame_observer.cpp)
+
 add_dependencies_and_linkings(avt_camera_nodelets)
 
 #############
@@ -143,7 +153,7 @@ install(DIRECTORY include
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 install(FILES
   plugins.xml
-  launch/mono_camera.launch
+  #launch/mono_camera.launch
   launch/mono_camera_nodelet.launch
   launch/stereo_camera_one_node.launch
   launch/stereo_camera_two_nodes.launch
@@ -179,3 +189,8 @@ elseif("${ARCH}" STREQUAL armv8)
     DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   )
 endif()
+#Test
+## gtest C++ Framework
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+catkin_add_gtest(${PROJECT_NAME}-test test/status_mako_camera_test.cpp)
+target_link_libraries(${PROJECT_NAME}-test avt_vimba_camera_lib ${catkin_LIBRARIES})

include/avt_vimba_camera/mono_camera.h

@@ -43,19 +43,22 @@
 #include <camera_info_manager/camera_info_manager.h>
 #include <image_transport/image_transport.h>
 #include <dynamic_reconfigure/server.h>
-
+#include <cav_msgs/SystemAlert.h>
+#include <cav_msgs/DriverStatus.h>
 #include <string>
 
 namespace avt_vimba_camera {
 class MonoCamera {
  public:
   MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp);
   ~MonoCamera(void);
+  void updateCameraStatus();
+  uint8_t cam_status;
+  private:
 
- private:
   AvtVimbaApi api_;
   AvtVimbaCamera cam_;
-
+  ros::Time last_time_;
   // diagnostic_updater::Updater updater_;
   // diagnostic_updater::TopicDiagnostic* pub_freq_;
 
@@ -71,8 +74,6 @@ class MonoCamera {
   // ROS Camera publisher
   image_transport::CameraPublisher pub_;
 
-
-
   // sensor_msgs::CameraInfo left_info_;
   boost::shared_ptr<camera_info_manager::CameraInfoManager> info_man_;
 

include/avt_vimba_camera/status_camera.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2019 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+#include <ros/ros.h>
+#include <avt_vimba_camera/mono_camera.h>
+#include <cav_msgs/SystemAlert.h>
+#include <cav_msgs/DriverStatus.h>
+#include <ros/callback_queue.h>
+#include <boost/thread.hpp>
+
+namespace avt_vimba_camera {
+class StatusCamera
+{
+    ros::Publisher status_pub_ ;
+    boost::thread* cam_thread_;
+
+public:
+    uint8_t status_cam;
+    cav_msgs::DriverStatus status_;
+    //Destructor to interrupt the cam_thread
+    ~StatusCamera();
+    void alertCallback(const cav_msgs::SystemAlertConstPtr &msg);
+    void publish_status();
+    void publish_off_status();
+    void pre_camera(ros::Publisher status_pub);
+    void post_camera();
+};
+}
+

launch/mono_camera.launch

@@ -5,8 +5,8 @@
 	</group>
 
 	<node name="camera" pkg="avt_vimba_camera" type="mono_camera_node" output="screen">
-		<param name="guid" value="50-0503343290"/>
-		<param name="ip" value=""/>
+		<param name="guid" value=""/>
+		<param name="ip" value="169.254.22.76"/>
 		<param name="camera_info_url" value="file://$(find avt_vimba_camera)/calibrations/calibration_50-0503343290.yaml"/>
 		<param name="frame_id" value="left_optical"/>
 		<param name="trig_timestamp_topic" value=""/>

package.xml

@@ -25,6 +25,7 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>polled_camera</build_depend>
   <build_depend>nodelet</build_depend>
+  <build_depend>cav_msgs</build_depend>
   <!--build_depend>libvimba</build_depend-->
   <run_depend>camera_info_manager</run_depend>
   <run_depend>diagnostic_updater</run_depend>
@@ -37,6 +38,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>polled_camera</run_depend>
   <run_depend>nodelet</run_depend>
+  <run_depend>cav_msgs</run_depend>
 
   <run_depend>image_proc</run_depend>
   <run_depend>stereo_image_proc</run_depend>

src/mono_camera.cpp

@@ -31,22 +31,21 @@
 /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <avt_vimba_camera/mono_camera.h>
-
 #define DEBUG_PRINTS 1
 
 namespace avt_vimba_camera {
 
 MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) {
   // Prepare node handle for the camera
   // TODO use nodelets with getMTNodeHandle()
-
   // Start Vimba & list all available cameras
   api_.start();
 
   // Set the image publisher before the streaming
   pub_  = it_.advertiseCamera("image_raw",  1);
 
   // Set the frame callback
+
   cam_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::frameCallback, this, _1));
 
   // Set the params
@@ -67,9 +66,11 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp
 MonoCamera::~MonoCamera(void) {
   cam_.stop();
   pub_.shutdown();
-}
+  }
 
 void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr) {
+   cam_status=cav_msgs::DriverStatus::OPERATIONAL;
+   last_time_=ros::Time::now();
   ros::Time ros_time = ros::Time::now();
   if (pub_.getNumSubscribers() > 0) {
     sensor_msgs::Image img;
@@ -78,8 +79,10 @@ void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr) {
       ci.header.stamp = img.header.stamp = ros_time;
       img.header.frame_id = ci.header.frame_id;
       pub_.publish(img, ci);
+
     } else {
       ROS_WARN_STREAM("Function frameToImage returned 0. No image published.");
+      cam_status=cav_msgs::DriverStatus::FAULT;
     }
   }
   // updater_.update();
@@ -162,4 +165,18 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig&
   info_man_->setCameraInfo(ci);
 }
 
+//time_compare compares the system time with time stamp and if its greater than 3 sec it assigns OFF status
+void MonoCamera::updateCameraStatus()
+{
+    ros::Duration img_time_difference=ros::Time::now()-last_time_;
+    //Greater than 2 seconds of non-response is the minimum time required to classify as not connected
+  if ((img_time_difference>ros::Duration(2.0)) && cam_status==cav_msgs::DriverStatus::OPERATIONAL)
+    {
+    cam_status=cav_msgs::DriverStatus::OFF;
+    }
+}
 };
+
+
+
+

src/nodes/mono_camera_node.cpp

@@ -1,15 +1,30 @@
 #include <ros/ros.h>
 #include <avt_vimba_camera/mono_camera.h>
+#include <avt_vimba_camera/status_camera.h>
 
 int main(int argc, char** argv)
 {
-	ros::init(argc, argv, "mono_camera_node");
-
+  ros::init(argc, argv, "mono_camera_node");
   ros::NodeHandle nh;
   ros::NodeHandle nhp("~");
-
+  ros::Rate loop_rate(19); //Camera max frame rate is 19Hz
+  avt_vimba_camera::StatusCamera hc;
+  ros::Publisher status_pub = nh.advertise<cav_msgs::DriverStatus>("driver_discovery", 1);
+  ros::Subscriber alert_sub = nh.subscribe<cav_msgs::SystemAlert>("system_alert",10,&avt_vimba_camera::StatusCamera::alertCallback, &hc);
+  hc.pre_camera(status_pub);
   avt_vimba_camera::MonoCamera mc(nh,nhp);
+  hc.post_camera();
 
-  ros::spin();
+while(ros::ok())
+{
+mc.updateCameraStatus();
+ros::spinOnce();
+loop_rate.sleep();
+hc.status_cam=mc.cam_status;
+hc.publish_status();
+status_pub.publish(hc.status_);
+}
   return 0;
-}
+}
+
+

src/status_camera.cpp

@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2019 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+#include <avt_vimba_camera/status_camera.h>
+
+namespace avt_vimba_camera {
+
+//Destructor to interrupt the cam_thread
+StatusCamera::~StatusCamera()
+{
+    cam_thread_->interrupt();
+}
+
+//System alert function definition
+void StatusCamera::alertCallback(const cav_msgs::SystemAlertConstPtr &msg)
+{
+  if( msg->type==cav_msgs::SystemAlert::FATAL || msg->type==cav_msgs::SystemAlert::SHUTDOWN)
+    {
+     ros::shutdown();
+    }
+}
+
+void StatusCamera::publish_status()
+{  //Various driver status conditions
+    boost::this_thread::interruption_point();
+    if (status_cam==cav_msgs::DriverStatus::OFF)
+    {
+     status_.status=cav_msgs::DriverStatus::OFF;
+    }
+    else if (status_cam==cav_msgs::DriverStatus::OPERATIONAL)
+    {
+    status_.status=cav_msgs::DriverStatus::OPERATIONAL;
+    }
+    else if (status_cam==cav_msgs::DriverStatus::FAULT)
+    {
+    status_.status=cav_msgs::DriverStatus::FAULT;
+    }
+}
+
+void StatusCamera::publish_off_status()
+{
+    while (true) {
+        boost::this_thread::interruption_point();
+        status_.status = cav_msgs::DriverStatus::OFF;
+        status_pub_.publish(status_);
+        ros::Duration(0.1).sleep();
+    }
+}
+
+//Start the thread and subscriber declaration
+void StatusCamera::pre_camera(ros::Publisher status_pub)
+{
+    status_.name=ros::this_node::getName();
+    status_.camera=true;
+    status_pub_ = status_pub;
+    cam_thread_ = new boost::thread(boost::bind(&StatusCamera::publish_off_status,this));
+}
+//Interrupt the cam_thread
+void StatusCamera::post_camera()
+{
+    cam_thread_->interrupt();
+}
+}