Commit fcef6454 authored by asantamaria's avatar asantamaria

Added thread

parent e514e024
......@@ -34,6 +34,9 @@
// Eigen stuff
#include <eigen3/Eigen/Dense>
// Boost stuff
#include <boost/thread/thread.hpp>
// [publisher subscriber headers]
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/Range.h>
......@@ -84,6 +87,12 @@ private:
bool px4_use_vxy_; // Use Vxy update. Flase uses Flow2D.
// Processing thread
boost::thread Thread_;
bool RunThread_;
void ThreadFunc(void);
// [publisher attributes]
ros::Publisher odom_publisher_;
nav_msgs::Odometry odom_msg_;
......
......@@ -58,6 +58,10 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
// [init action servers]
// [init action clients]
// Initialize Filter Thread
Thread_ = boost::thread( boost::bind(&EskfOdomAlgNode::ThreadFunc, this) );
RunThread_ = true;
}
EskfOdomAlgNode::~EskfOdomAlgNode(void)
......@@ -68,6 +72,9 @@ EskfOdomAlgNode::~EskfOdomAlgNode(void)
pthread_mutex_destroy(&this->flying_mutex_);
pthread_mutex_destroy(&this->px4_of_mutex_);
pthread_mutex_destroy(&this->imu_mutex_);
RunThread_ = false;
Thread_.join();
}
void EskfOdomAlgNode::read_and_set_ini_params(void)
......@@ -209,14 +216,6 @@ Eigen::VectorXf EskfOdomAlgNode::read_vec(const std::string& param_name,const in
void EskfOdomAlgNode::mainNodeThread(void)
{
// // DEBUG: Frequency check
// this->ros_t_curr_ = ros::Time::now();
// double t_last = (this->ros_t_last_ - this->ros_t_init_).toSec();
// double t_curr = (this->ros_t_curr_ - this->ros_t_init_).toSec();
// double dt = t_curr - t_last;
// this->ros_t_last_ = this->ros_t_curr_;
// std::cout << "Current freq: " << 1/dt << " Hz" << std::endl;
// [fill msg structures]
// [fill srv structure and make request to the server]
......@@ -224,50 +223,64 @@ 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();
Eigen::VectorXf state(33,1);
Eigen::Vector3f ang_vel;
bool step_done = false;
this->alg_.lock();
step_done = this->alg_.update(state,ang_vel,is_flying,this->range_dist_(0));
this->alg_.unlock();
if (step_done)
{
// Broadcast state with TF
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
tf::Quaternion q(state(7), state(8), state(9), state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), this->odom_frame_id_, this->robot_frame_id_));
// Publish Odometry
this->odom_msg_.header.seq = this->seq_;
this->odom_msg_.header.stamp = ros::Time::now();
this->odom_msg_.header.frame_id = this->odom_frame_id_;
this->odom_msg_.child_frame_id = this->robot_frame_id_;
this->odom_msg_.pose.pose.position.x = state(0);
this->odom_msg_.pose.pose.position.y = state(1);
this->odom_msg_.pose.pose.position.z = state(2);
this->odom_msg_.pose.pose.orientation.w = state(6);
this->odom_msg_.pose.pose.orientation.x = state(7);
this->odom_msg_.pose.pose.orientation.y = state(8);
this->odom_msg_.pose.pose.orientation.z = state(9);
this->odom_msg_.twist.twist.linear.x = state(3);
this->odom_msg_.twist.twist.linear.y = state(4);
this->odom_msg_.twist.twist.linear.z = state(5);
this->odom_msg_.twist.twist.angular.x = ang_vel(0);
this->odom_msg_.twist.twist.angular.y = ang_vel(1);
this->odom_msg_.twist.twist.angular.z = ang_vel(2);
this->odom_publisher_.publish(this->odom_msg_);
++this->seq_;
};
void EskfOdomAlgNode::ThreadFunc(void)
{
while(RunThread_)
{
// // DEBUG: Frequency check
// this->ros_t_curr_ = ros::Time::now();
// double t_last = (this->ros_t_last_ - this->ros_t_init_).toSec();
// double t_curr = (this->ros_t_curr_ - this->ros_t_init_).toSec();
// double dt = t_curr - t_last;
// this->ros_t_last_ = this->ros_t_curr_;
// std::cout << "Current freq: " << 1/dt << " Hz" << std::endl;
this->flying_mutex_enter();
bool is_flying = this->flying_;
this->flying_mutex_exit();
Eigen::VectorXf state(33,1);
Eigen::Vector3f ang_vel;
bool step_done = false;
this->alg_.lock();
step_done = this->alg_.update(state,ang_vel,is_flying,this->range_dist_(0));
this->alg_.unlock();
if (step_done)
{
// Broadcast state with TF
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
tf::Quaternion q(state(7), state(8), state(9), state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), this->odom_frame_id_, this->robot_frame_id_));
// Publish Odometry
this->odom_msg_.header.seq = this->seq_;
this->odom_msg_.header.stamp = ros::Time::now();
this->odom_msg_.header.frame_id = this->odom_frame_id_;
this->odom_msg_.child_frame_id = this->robot_frame_id_;
this->odom_msg_.pose.pose.position.x = state(0);
this->odom_msg_.pose.pose.position.y = state(1);
this->odom_msg_.pose.pose.position.z = state(2);
this->odom_msg_.pose.pose.orientation.w = state(6);
this->odom_msg_.pose.pose.orientation.x = state(7);
this->odom_msg_.pose.pose.orientation.y = state(8);
this->odom_msg_.pose.pose.orientation.z = state(9);
this->odom_msg_.twist.twist.linear.x = state(3);
this->odom_msg_.twist.twist.linear.y = state(4);
this->odom_msg_.twist.twist.linear.z = state(5);
this->odom_msg_.twist.twist.angular.x = ang_vel(0);
this->odom_msg_.twist.twist.angular.y = ang_vel(1);
this->odom_msg_.twist.twist.angular.z = ang_vel(2);
this->odom_publisher_.publish(this->odom_msg_);
++this->seq_;
}
}
}
/* [subscriber 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