added hl_status to use asctec_mav_framework

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm asctec_msgs px_comm sensor_msgs tf)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm asctec_hl_comm asctec_msgs px_comm sensor_msgs tf)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm asctec_msgs px_comm sensor_msgs tf
+ CATKIN_DEPENDS iri_base_algorithm asctec_hl_comm asctec_msgs px_comm sensor_msgs tf
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -96,6 +96,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${eskf_odometry_LIBRAR
 #               Add message headers dependencies 
 # ******************************************************************** 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} asctec_hl_comm_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} asctec_msgs_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} px_comm_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)

include/eskf_odom_alg_node.h

@@ -39,6 +39,7 @@
 
 
 // [publisher subscriber headers]
+#include <asctec_hl_comm/mav_status.h>
 #include <asctec_msgs/LLStatus.h>
 #include <px_comm/OpticalFlow.h>
 #include <sensor_msgs/Imu.h>
@@ -77,6 +78,11 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
     // [publisher attributes]
 
     // [subscriber attributes]
+    ros::Subscriber hl_status_subscriber_;
+    void hl_status_callback(const asctec_hl_comm::mav_status::ConstPtr& msg);
+    pthread_mutex_t hl_status_mutex_;
+    void hl_status_mutex_enter(void);
+    void hl_status_mutex_exit(void);
     ros::Subscriber ll_status_subscriber_;
     void ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg);
     pthread_mutex_t ll_status_mutex_;

package.xml

@@ -9,6 +9,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>iri_base_algorithm</build_depend>
+  <build_depend>asctec_hl_comm</build_depend>
   <build_depend>asctec_msgs</build_depend>
   <build_depend>px_comm</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -16,6 +17,7 @@
   <build_depend>eskf_odometry</build_depend>
 
   <run_depend>iri_base_algorithm</run_depend>
+  <run_depend>asctec_hl_comm</run_depend>
   <run_depend>asctec_msgs</run_depend>
   <run_depend>px_comm</run_depend>
   <run_depend>tf</run_depend>

src/eskf_odom_alg_node.cpp

@@ -23,6 +23,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
   // [init publishers]
   
   // [init subscribers]
+  this->hl_status_subscriber_ = this->public_node_handle_.subscribe("hl_status", 1, &EskfOdomAlgNode::hl_status_callback, this); 
+  pthread_mutex_init(&this->hl_status_mutex_,NULL);
+
 
   this->ll_status_subscriber_ = this->public_node_handle_.subscribe("ll_status", 1, &EskfOdomAlgNode::ll_status_callback, this); 
   pthread_mutex_init(&this->ll_status_mutex_,NULL);
@@ -106,6 +109,7 @@ EskfOdomAlgNode::~EskfOdomAlgNode(void)
 {
 #ifndef READ_FROM_FILE 
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->hl_status_mutex_);
   pthread_mutex_destroy(&this->ll_status_mutex_);
   pthread_mutex_destroy(&this->px4_of_mutex_);
   pthread_mutex_destroy(&this->imu_mutex_);
@@ -355,6 +359,27 @@ void EskfOdomAlgNode::mainNodeThread(void)
 #ifndef READ_FROM_FILE
 
   /*  [subscriber callbacks] */
+void EskfOdomAlgNode::hl_status_callback(const asctec_hl_comm::mav_status::ConstPtr& msg) 
+{ 
+  ROS_INFO("EskfOdomAlgNode::hl_status_callback: New Message Received"); 
+
+  this->hl_status_mutex_enter();
+  if (msg->motor_status.compare("running") == 0)
+	  this->flying_ = true;
+  else
+	  this->flying_ = false;
+  this->hl_status_mutex_exit();
+} 
+
+void EskfOdomAlgNode::hl_status_mutex_enter(void) 
+{ 
+  pthread_mutex_lock(&this->hl_status_mutex_); 
+} 
+
+void EskfOdomAlgNode::hl_status_mutex_exit(void) 
+{ 
+  pthread_mutex_unlock(&this->hl_status_mutex_); 
+}
   void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg) 
   { 
     ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");