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,14 +107,14 @@ 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)
 

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();
+ short int cam_status;
  private:
+
   AvtVimbaApi api_;
   AvtVimbaCamera cam_;
-
+  ros::Time last_time_;
   // diagnostic_updater::Updater updater_;
   // diagnostic_updater::TopicDiagnostic* pub_freq_;
 
@@ -67,6 +70,7 @@ class MonoCamera {
   std::string camera_info_url_;
   bool show_debug_prints_;
 
+
   image_transport::ImageTransport it_;
   // ROS Camera publisher
   image_transport::CameraPublisher pub_;

include/avt_vimba_camera/status_camera.h

@@ -0,0 +1,27 @@
+#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>
+
+class StatusCamera
+{
+    cav_msgs::DriverStatus status_;
+    ros::Publisher status_pub_ ;
+    ros::Subscriber alert_sub_;
+    ros::NodeHandle nh_;
+    ros::NodeHandle nhp_;
+    boost::thread* cam_thread_;
+public:
+    short int status_cam;
+    //Constructor
+    StatusCamera(ros::NodeHandle,ros::NodeHandle);
+    //Destructor to interrupt the cam_thread
+    ~StatusCamera();
+    void alertCallback(const cav_msgs::SystemAlertConstPtr &msg);
+    void publish_status();
+    void pre_camera();
+    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,13 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp
 MonoCamera::~MonoCamera(void) {
   cam_.stop();
   pub_.shutdown();
-}
+  cam_status=cav_msgs::DriverStatus::OFF;
+
+ }
 
 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 +81,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 +167,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_;
+    ros::Duration three_seconds(2.0);
+    if ((img_time_difference>three_seconds) && cam_status==cav_msgs::DriverStatus::OPERATIONAL)
+{
+    cam_status=cav_msgs::DriverStatus::OFF;
+}
+}
 };
+
+
+
+

src/nodes/mono_camera_node.cpp

@@ -1,15 +1,25 @@
 #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(10);
+  StatusCamera hc(nh,nhp);
+  hc.pre_camera();
   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;
+}
   return 0;
-}
+}
+

src/status_camera.cpp

@@ -0,0 +1,56 @@
+#include <avt_vimba_camera/status_camera.h>
+
+StatusCamera::StatusCamera(ros::NodeHandle nh, ros::NodeHandle nhp)
+{
+    status_.name=ros::this_node::getName();
+    status_.camera=true;
+    nh_=nh;
+    nhp_=nhp;
+    //Publisher object for driver_discovery
+    status_pub_ = nh_.advertise<cav_msgs::DriverStatus>("driver_discovery", 1);
+}
+//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
+    while(true){
+        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;
+       }
+       //Publisher for driver_status
+       status_pub_.publish(status_);
+       ros::Duration(0.1).sleep();
+    }
+}
+//Start the thread and subscriber declaration
+void StatusCamera::pre_camera()
+{
+    cam_thread_ = new boost::thread(boost::bind(&StatusCamera::publish_status,this));
+    alert_sub_ = nh_.subscribe<cav_msgs::SystemAlert>("system_alert",10,&StatusCamera::alertCallback, this);
+}
+//Interrupt the cam_thread
+void StatusCamera::post_camera()
+{
+    cam_thread_->interrupt();
+
+}