Commit a6725bfc authored by asantamaria's avatar asantamaria

removed asctec dependencies. Created a new node kinton_eskf_odom

parent 968200a8
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm roslib asctec_hl_comm asctec_msgs px_comm sensor_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm roscpp roslib px_comm sensor_msgs tf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -62,11 +62,11 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
CATKIN_DEPENDS iri_base_algorithm roslib asctec_hl_comm asctec_msgs px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm roscpp roslib px_comm sensor_msgs tf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
# DEPENDS eskf_odometry
DEPENDS eskf_odometry atools
)
###########
......@@ -96,8 +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} roscpp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} px_comm_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
# ********************************************************************
......
This diff is collapsed.
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
This diff is collapsed.
......@@ -39,15 +39,11 @@
#include <eigen3/Eigen/Dense>
#include <ros/package.h>
// [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>
// [service client headers]
#include <roscpp/Empty.h>
// [action server client headers]
......@@ -66,7 +62,6 @@ private:
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
#ifndef READ_FROM_FILE
bool is_first_imu_; // First IMU reading should not contribute to propagate nominal (integration requirements).
// Also used to get initial sensor time.
bool is_first_px4_; // Used to get initial sensor time.
......@@ -88,16 +83,6 @@ private:
// [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_;
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_;
......@@ -114,9 +99,13 @@ private:
void set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
const double& msg_time);
#endif
// [service attributes]
ros::ServiceServer flying_server_;
bool flyingCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res);
pthread_mutex_t flying_mutex_;
void flying_mutex_enter(void);
void flying_mutex_exit(void);
// [client attributes]
......@@ -124,42 +113,6 @@ private:
// [action client attributes]
#ifdef READ_FROM_FILE
int imu_idx_;
int px4_idx_;
int llstatus_idx_;
double t_ini_imu_;
double t_ini_px4_;
double t_ini_llstatus_;
std::string data_path_;
double max_freq_;
vector <vector<double>> a_;
vector <vector<double>> w_;
vector <vector<double>> px4_;
vector <vector<double>> llstatus_;
/**
* \brief Read all data from files
*
* This function read all sensor and robot state data from files and
* sets them into the filter.
*/
void read_all_data_from_files(void);
/**
* \brief Read data from files
*
* This function reads data from a .txt file (ROS Matlab format).
*/
vector <vector<double>> read_from_file(const string& path);
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......
......@@ -3,7 +3,6 @@
<node name="eskf_odom" pkg="eskf_odom" type="eskf_odom" output="screen">
<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>
......
......@@ -24,6 +24,3 @@ xstate0: [0.0,0.0,0.1,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,-0.0005,-0.0026,-0
# Initial Error-State vector (dp,dv,dtheta,dab,dwb,dg)
dxstate0: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# Read from files in path:
data_path: data/kinton_outdoor_paper/2
......@@ -9,22 +9,18 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</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>
<build_depend>tf</build_depend>
<build_depend>eskf_odometry</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</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>
<run_depend>sensor_msgs</run_depend>
<run_depend>eskf_odometry</run_depend>
<export>
</export>
......
......@@ -7,13 +7,11 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
{
this->flying_ = false;
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
// Read initial parameters from yaml file and set themn to sensors and filter
read_and_set_ini_params();
#ifndef READ_FROM_FILE
this->loop_rate_ = 1000; //in [Hz]
// Initialize vars
......@@ -28,25 +26,15 @@ 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);
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 10,&EskfOdomAlgNode::px4_of_callback, this);
pthread_mutex_init(&this->px4_of_mutex_, NULL);
this->imu_subscriber_ = this->public_node_handle_.subscribe("imu", 10,&EskfOdomAlgNode::imu_callback, this);
pthread_mutex_init(&this->imu_mutex_, NULL);
#endif
#ifdef READ_FROM_FILE
read_all_data_from_files();
#endif
// [init services]
this->flying_server_ = this->public_node_handle_.advertiseService("flying", &EskfOdomAlgNode::flyingCallback, this);
pthread_mutex_init(&this->flying_mutex_,NULL);
// [init clients]
......@@ -55,42 +43,12 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
// [init action clients]
}
#ifdef READ_FROM_FILE
void EskfOdomAlgNode::read_all_data_from_files(void)
{
this->imu_idx_ = 0.0;
this->px4_idx_ = 0.0;
this->llstatus_idx_ = 0.0;
std::string node_path = ros::package::getPath("eskf_odom");
this->a_ = read_from_file(node_path + "/" + this->data_path_ + "/linear_acceleration.txt");
this->w_ = read_from_file(node_path + "/" + this->data_path_ +"/angular_velocity.txt");
this->px4_ = read_from_file(node_path + "/" + this->data_path_ +"/opt_flow.txt");
this->llstatus_ = read_from_file(node_path + "/" + this->data_path_ +"/ll_status.txt");
this->t_ini_imu_ = this->a_[0][0];
this->t_ini_px4_ = this->px4_[0][0];
this->t_ini_llstatus_ = this->llstatus_[0][0];
// Find message with max frequency to set loop rate
double imu_freq = this->a_.size()/((this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9);
double px4_freq = this->px4_.size()/((this->px4_[this->px4_.size()-1][0]-this->t_ini_px4_)*1e-9);
double llstatus_freq = this->llstatus_.size()/((this->llstatus_[this->llstatus_.size()-1][0]-this->t_ini_llstatus_)*1e-9);
this->max_freq_ = max(max(imu_freq,px4_freq),llstatus_freq);
this->loop_rate_ = this->max_freq_;//in [Hz]
}
#endif
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->flying_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
#endif
}
void EskfOdomAlgNode::read_and_set_ini_params(void)
......@@ -149,17 +107,13 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
// Set filter and sensors initial parameters
this->alg_.set_init_params(f_params, x_state, dx_state, imu_params, px4_params);
#ifdef READ_FROM_FILE
this->public_node_handle_.param<string>("data_path", this->data_path_, "");
#endif
// Print already set values of filter and sensors initial parameters
//this->alg_.print_ini_params();
this->alg_.unlock();
}
Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,
const int& exp_long)
Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,const int& exp_long)
{
Eigen::VectorXd params = Eigen::VectorXd::Zero(exp_long);
......@@ -183,43 +137,6 @@ Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,
return params;
}
#ifdef READ_FROM_FILE
vector <vector<double>> EskfOdomAlgNode::read_from_file(const string& path)
{
vector <vector<double>> data;
ifstream infile(path.c_str());
string s;
getline(infile,s);
while (infile)
{
if (!getline(infile,s)) break;
istringstream ss(s);
vector<double> record;
while (ss)
{
string s;
if (!getline( ss, s, ',' )) break;
record.push_back(atof(s.c_str()));
}
data.push_back(record);
}
if (!infile.eof())
{
cerr << "Error reading file " << path.c_str() << "\n";
}
return data;
}
#endif
void EskfOdomAlgNode::mainNodeThread(void)
{
......@@ -230,84 +147,21 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [fill action structure and make request to the action server]
// [publish messages]
this->flying_mutex_enter();
bool is_flying = this->flying_;
this->flying_mutex_exit();
this->alg_.lock();
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
this->alg_.set_obs_phase(is_flying,this->gnd_dist_);
this->alg_.unlock();
Eigen::VectorXd state(19,1);
bool step_done = false;
#ifdef READ_FROM_FILE
// IMU propagation ***************
ROS_DEBUG("IMU process");
double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
double ax = this->a_[this->imu_idx_][1];
double ay = -this->a_[this->imu_idx_][2]; // axis change according to simulation
double az = -this->a_[this->imu_idx_][3];// axis change according to simulation
double wx = this->w_[this->imu_idx_][1];
double wy = -this->w_[this->imu_idx_][2];// axis change according to simulation
double wz = -this->w_[this->imu_idx_][3];// axis change according to simulation
Eigen::Vector3d a;
a << ax,ay,az;
Eigen::Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
this->alg_.set_imu_reading(t_imu,a,w);// Set values into filter object
step_done = this->alg_.run_next_step(state); // Propagate Nominal-State
this->alg_.unlock();
++this->imu_idx_;
double next_t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
// PX4 Innovation ***************
double t_px4 = (this->px4_[this->px4_idx_][0]-t_ini_px4_)*1e-9;
ROS_DEBUG("process PX4");
this->gnd_dist_ = this->px4_[this->px4_idx_][4];
double vx = this->px4_[this->px4_idx_][7];
double vy = -this->px4_[this->px4_idx_][8]; // axis change according to simulation
double flowx = this->px4_[this->px4_idx_][5];
double flowy = -this->px4_[this->px4_idx_][6];// axis change according to simulation
Eigen::Vector3d val;
val << this->gnd_dist_,vx,vy;
Eigen::Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
this->alg_.set_px4_reading(t_px4,val,flow);// Set values into filter object
step_done = this->alg_.run_next_step(state); // Propagate Nominal-State
this->alg_.unlock();
++this->px4_idx_;
// LLStatus process ****************
double t_llstatus = (this->llstatus_[this->llstatus_idx_][0]-t_ini_llstatus_)*1e-9;
if (this->llstatus_idx_ < this->llstatus_.size() && t_llstatus < next_t_imu)
{
ROS_DEBUG("process ll_status");
this->flying_ = this->llstatus_[this->llstatus_idx_][10];
++this->llstatus_idx_;
}
#endif
#ifndef READ_FROM_FILE
this->alg_.lock();
step_done = this->alg_.run_next_step(state);
this->alg_.unlock();
#endif
if (step_done)
{
// Broadcast state with TF
......@@ -320,50 +174,11 @@ void EskfOdomAlgNode::mainNodeThread(void)
};
}
#ifndef READ_FROM_FILE
/* [subscriber callbacks] */
void EskfOdomAlgNode::hl_status_callback(
const asctec_hl_comm::mav_status::ConstPtr& msg)
{
ROS_DEBUG("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");
this->ll_status_mutex_enter();
this->flying_ = msg->flying;
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_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
......@@ -386,7 +201,7 @@ void EskfOdomAlgNode::px4_of_mutex_exit(void)
void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
// ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
ros::Time stamp = msg->header.stamp;
double t = stamp.toSec();
......@@ -453,9 +268,30 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
this->alg_.unlock();
}
#endif
/* [service callbacks] */
bool EskfOdomAlgNode::flyingCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res)
{
ROS_DEBUG("EskfOdomAlgNode::flyingCallback: New Request Received!");
this->flying_mutex_enter();
if (this->flying_)
this->flying_ = false;
else
this->flying_ = true;
this->flying_mutex_exit();
return true;
}
void EskfOdomAlgNode::flying_mutex_enter(void)
{
pthread_mutex_lock(&this->flying_mutex_);
}
void EskfOdomAlgNode::flying_mutex_exit(void)
{
pthread_mutex_unlock(&this->flying_mutex_);
}
/* [action callbacks] */
......
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