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)
@@ -89,12 +92,14 @@ function(add_dependencies_and_linkings arg)
   endif()
 endfunction(add_dependencies_and_linkings)
 
+#file(GLOB_RECURSE headers */*.hpp */*.h)
+
 add_executable(mono_camera_node
   src/nodes/mono_camera_node.cpp
   src/mono_camera.cpp
   src/avt_vimba_camera.cpp
   src/frame_observer.cpp
-)
+    )
 
 add_dependencies_and_linkings(mono_camera_node)
 
@@ -103,14 +108,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,16 +43,20 @@
 #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 publish_status();
+ void alertCallback(const cav_msgs::SystemAlertConstPtr &msg);
+ int flag;
  private:
+  cav_msgs::DriverStatus status_;
   AvtVimbaApi api_;
   AvtVimbaCamera cam_;
 
@@ -67,6 +71,8 @@ class MonoCamera {
   std::string camera_info_url_;
   bool show_debug_prints_;
 
+  ros::Publisher status_pub_;
+  ros::Subscriber alert_sub_;
   image_transport::ImageTransport it_;
   // ROS Camera publisher
   image_transport::CameraPublisher pub_;

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

@@ -1,3 +1,5 @@
+
+
 /// Copyright (c) 2014,
 /// Systems, Robotics and Vision Group
 /// University of the Balearic Islands
@@ -31,21 +33,22 @@
 /// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <avt_vimba_camera/mono_camera.h>
-
+#include <iostream>
 #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);
 
+  status_pub_ = nh_.advertise<cav_msgs::DriverStatus>("driver_discovery", 1);
+  alert_sub_ = nh_.subscribe("system_alert",10,&MonoCamera::alertCallback,this);
   // Set the frame callback
   cam_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::frameCallback, this, _1));
 
@@ -59,27 +62,41 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp
 
   // Set camera info manager
   info_man_  = boost::shared_ptr<camera_info_manager::CameraInfoManager>(new camera_info_manager::CameraInfoManager(nhp_, frame_id, camera_info_url_));
+//issue
 
+  status_.name=ros::this_node::getName();
+  status_.camera=true;
+
+flag=1;
   // Start dynamic_reconfigure & run configure()
   reconfigure_server_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::configure, this, _1, _2));
+
+
 }
 
 MonoCamera::~MonoCamera(void) {
   cam_.stop();
   pub_.shutdown();
-}
+  flag=1;
+
+ }
 
 void MonoCamera::frameCallback(const FramePtr& vimba_frame_ptr) {
   ros::Time ros_time = ros::Time::now();
   if (pub_.getNumSubscribers() > 0) {
     sensor_msgs::Image img;
+
     if (api_.frameToImage(vimba_frame_ptr, img)) {
+
       sensor_msgs::CameraInfo ci = info_man_->getCameraInfo();
       ci.header.stamp = img.header.stamp = ros_time;
       img.header.frame_id = ci.header.frame_id;
       pub_.publish(img, ci);
+      flag=2;
+
     } else {
       ROS_WARN_STREAM("Function frameToImage returned 0. No image published.");
+      flag=3;
     }
   }
   // updater_.update();
@@ -162,4 +179,37 @@ void MonoCamera::updateCameraInfo(const avt_vimba_camera::AvtVimbaCameraConfig&
   info_man_->setCameraInfo(ci);
 }
 
+void MonoCamera::publish_status()
+{
+
+ if (flag==1)
+{
+    ROS_INFO_STREAM("1");
+status_.status=cav_msgs::DriverStatus::OFF;
+}
+else if (flag==2)
+{
+status_.status=cav_msgs::DriverStatus::OPERATIONAL;
+}
+//status_.status=cav_msgs::DriverStatus::DEGRADED;
+else if (flag==3)
+{
+status_.status=cav_msgs::DriverStatus::FAULT;
+}
+status_pub_.publish(status_);
+}
+
+void MonoCamera::alertCallback(const cav_msgs::SystemAlertConstPtr &msg)
+{
+if( msg->type==cav_msgs::SystemAlert::FATAL || msg->type==cav_msgs::SystemAlert::SHUTDOWN)
+{
+ros::shutdown();
+}
+
+}
+
 };
+
+
+
+

src/nodes/mono_camera_node.cpp

@@ -7,9 +7,15 @@ int main(int argc, char** argv)
 
   ros::NodeHandle nh;
   ros::NodeHandle nhp("~");
-
+  ros::Rate loop_rate(10);
   avt_vimba_camera::MonoCamera mc(nh,nhp);
-
-  ros::spin();
+
+ // ros::spin();
+while(ros::ok())
+{
+mc.publish_status();
+ros::spinOnce();
+loop_rate.sleep();
+}
   return 0;
-}
+}