Commit d622e3f5 authored by adrianamor's avatar adrianamor

Params initialized. Set quadrotor phase. TODO low-level phase and IMU and PX4 data set

parent f7f81d31
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm px_comm sensor_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm 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 px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm asctec_msgs px_comm sensor_msgs tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
......@@ -95,6 +95,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_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} px_comm_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
......
......@@ -31,6 +31,8 @@
//include eskf_odom_alg main library
const double PX4_MIN_HEIGHT = 0.31;
/**
* \brief IRI ROS Specific Driver Class
*
......@@ -137,6 +139,22 @@ class EskfOdomAlgorithm
*/
void print_ini_params(void);
/**
* \brief Set Observation phase
*
* Sets the observation values depending on the robot phase.
* This step is required due to px4 limited range (0.3m<pz<5m)
* Different values for the sensor:
* -LANDED: sinthetic values (robot not moving).
* -TOFF,LANDING: High covariance in order to reduce update stage
* -FLYING: sensor readings and corresponding covariances.
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void set_obs_phase(const bool& flying, const double& gnd_dist);
/**
* \brief Destructor
*
......
......@@ -29,6 +29,7 @@
#include "eskf_odom_alg.h"
// [publisher subscriber headers]
#include <asctec_msgs/LLStatus.h>
#include <px_comm/OpticalFlow.h>
#include <sensor_msgs/Imu.h>
......@@ -49,9 +50,17 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
{
private:
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
// [publisher attributes]
// [subscriber attributes]
ros::Subscriber ll_status_subscriber_;
void ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg);
pthread_mutex_t ll_status_mutex_;
void ll_status_mutex_enter(void);
void ll_status_mutex_exit(void);
ros::Subscriber px4_of_subscriber_;
void px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg);
pthread_mutex_t px4_of_mutex_;
......
<launch>
<node name="eskf_odom" pkg="eskf_odom" type="eskf_odom" output="screen">
<remap from="imu" to="/imu" />
<remap from="px4_of" to="/px4_of" />
<remap from="~imu" to="/imu" />
<remap from="~px4_of" to="/px4flow/opt_flow" />
<remap from="~ll_status" to="/asctec/LL_STATUS" />
<rosparam file="$(find eskf_odom)/launch/params.yaml"/>
</node>
......
......@@ -9,12 +9,14 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>asctec_msgs</build_depend>
<build_depend>px_comm</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>eskf_odometry</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>asctec_msgs</run_depend>
<run_depend>px_comm</run_depend>
<run_depend>tf</run_depend>
<run_depend>sensor_msgs</run_depend>
......
......@@ -3,6 +3,10 @@
EskfOdomAlgorithm::EskfOdomAlgorithm(void)
{
pthread_mutex_init(&this->access_,NULL);
// Set initial robot phase as landed
this->filter_.set_robot_phase(Robot::LANDED);
this->filter_.print_robot_phase();
}
EskfOdomAlgorithm::~EskfOdomAlgorithm(void)
......@@ -28,4 +32,42 @@ void EskfOdomAlgorithm::set_init_params(const params& f_params, const VectorXd&
void EskfOdomAlgorithm::print_ini_params()
{
this->filter_.print_ini_params();
}
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist)
{
Robot::kinton_phases phase;
this->filter_.get_robot_phase(phase);
switch (phase) {
case Robot::LANDED:
if (flying){
this->filter_.set_robot_phase(Robot::TOFF);
this->filter_.print_robot_phase();
}
break;
case Robot::TOFF:
if (gnd_dist > PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::FLYING);
this->filter_.print_robot_phase();
}
break;
case Robot::FLYING:
if (gnd_dist < PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::LANDING);
this->filter_.print_robot_phase();
}
break;
case Robot::LANDING:
if (!flying){
this->filter_.set_robot_phase(Robot::LANDED);
this->filter_.print_robot_phase();
}
else
if (gnd_dist > PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::FLYING);
this->filter_.print_robot_phase();
}
break;
}
}
\ No newline at end of file
#include "eskf_odom_alg_node.h"
//TODO: Quadrotor phases (taking-off, flying, landing, landed)
//this means adding subscription to low-level quadrotor driver
//TODO: LOW-LEVEL LIBRARY
// Quadrotor phases (taking-off, flying, landing, landed)
// this means adding subscription to low-level quadrotor driver
//TODO: NODE
// SET IMU and PX4 data
using namespace std;
using namespace Eigen;
......@@ -12,9 +16,16 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// Initialize ROBOT phase
this->flying_ = false;
this->gnd_dist_ = PX4_MIN_HEIGHT; // a little bit more than the minimum distance PX4 can detect (0.3m)
// [init publishers]
// [init subscribers]
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);
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 1, &EskfOdomAlgNode::px4_of_callback, this);
pthread_mutex_init(&this->px4_of_mutex_,NULL);
......@@ -36,6 +47,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
EskfOdomAlgNode::~EskfOdomAlgNode(void)
{
// [free dynamic memory]
pthread_mutex_destroy(&this->ll_status_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
}
......@@ -120,22 +132,56 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
// Set Quadrotor State
this->ll_status_mutex_enter();
this->px4_of_mutex_enter();
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
this->ll_status_mutex_exit();
this->px4_of_mutex_exit();
}
/* [subscriber callbacks] */
void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->ll_status_mutex_enter();
this->flying_ = msg->flying;
//unlock previously blocked shared variables
//this->alg_.unlock();
this->ll_status_mutex_exit();
}
void EskfOdomAlgNode::ll_status_mutex_enter(void)
{
pthread_mutex_lock(&this->ll_status_mutex_);
}
void EskfOdomAlgNode::ll_status_mutex_exit(void)
{
pthread_mutex_unlock(&this->ll_status_mutex_);
}
void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
ROS_INFO("EskfOdomAlgNode::px4_of_callback: New Message Received");
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//this->px4_of_mutex_enter();
this->px4_of_mutex_enter();
//std::cout << msg->data << std::endl;
this->gnd_dist_ = msg->ground_distance;
//unlock previously blocked shared variables
//this->alg_.unlock();
//this->px4_of_mutex_exit();
this->px4_of_mutex_exit();
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
......@@ -149,14 +195,12 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
}
void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
ROS_INFO("EskfOdomAlgNode::imu_callback: New Message Received");
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//this->imu_mutex_enter();
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
//this->imu_mutex_exit();
......
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