Commit 579512b1 authored by asctec's avatar asctec
parents 7dd80272 f22a2ebd
......@@ -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)
......
......@@ -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_;
......
......@@ -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>
......
......@@ -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");
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment