Commit 585bd696 authored by asantamaria's avatar asantamaria

added odometry and cleaned namespaces and includes

parent a6725bfc
......@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm roscpp roslib px_comm sensor_msgs tf)
find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs roscpp roslib 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 roscpp roslib px_comm sensor_msgs tf
CATKIN_DEPENDS iri_base_algorithm nav_msgs roscpp roslib 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} nav_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)
......
......@@ -27,10 +27,9 @@
#include <eskf_odom/EskfOdomConfig.h>
// Main eskf library
#include <eskf_odometry.h>
//include eskf_odom_alg main library
/**
* \brief IRI ROS Specific Driver Class
*
......@@ -195,8 +194,9 @@ class EskfOdomAlgorithm
*
* Input/Ouput:
* state: Robot state (6D).
* ang_vel: Robot Angular Velocity (currently not included in the state).
*/
bool run_next_step(Eigen::VectorXd& state);
bool run_next_step(Eigen::VectorXd& state, Eigen::Vector3d& ang_vel);
/**
* \brief Destructor
......
......@@ -28,17 +28,14 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "eskf_odom_alg.h"
#include <fstream>
#include <iostream>
#include <sstream>
// STD stuff
#include <string>
//TODO: delete this queue
#include <queue> // std::queue
#include <tf/transform_broadcaster.h>
// Eigen stuff
#include <eigen3/Eigen/Dense>
// [publisher subscriber headers]
#include <nav_msgs/Odometry.h>
#include <px_comm/OpticalFlow.h>
#include <sensor_msgs/Imu.h>
......@@ -47,10 +44,6 @@
// [action server client headers]
using namespace std;
// #define READ_FROM_FILE
/**
* \brief IRI ROS Specific Algorithm Class
*
......@@ -62,8 +55,7 @@ private:
bool flying_; // Quadrotor landed or flying information.
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
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_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.
bool new_imu_; // Flag indicating a new IMU message is received.
......@@ -72,15 +64,13 @@ private:
double t_ini_px4_; // Initial PX4 sensor time.
double t_ini_imu_; // Initial IMU sensor time.
//TODO: delete this doubles
double tlast_imu_;
double tlast_px4_;
//TODO: delete this queues
std::queue<sensor_msgs::Imu::ConstPtr> imu_queue_;
std::queue<px_comm::OpticalFlow::ConstPtr> px4_queue_;
int seq_; // Odometry sequence counter.
std::string odom_frame_id_; // Odometry frame ID.
std::string robot_frame_id_; // Robot frame ID.
// [publisher attributes]
ros::Publisher odom_publisher_;
nav_msgs::Odometry odom_msg_;
// [subscriber attributes]
ros::Subscriber px4_of_subscriber_;
......@@ -88,24 +78,21 @@ private:
pthread_mutex_t px4_of_mutex_;
void px4_of_mutex_enter(void);
void px4_of_mutex_exit(void);
void set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,const double& msg_time);
ros::Subscriber imu_subscriber_;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
pthread_mutex_t imu_mutex_;
void imu_mutex_enter(void);
void imu_mutex_exit(void);
void set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,
const double& msg_time);
void set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
const double& msg_time);
void set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const double& msg_time);
// [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);
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]
......@@ -186,7 +173,7 @@ protected:
*
* This function reads a 3-Vector parameter specified by param_name
*/
Eigen::VectorXd read_vec(const string& param_name, const int& exp_long);
Eigen::VectorXd read_vec(const std::string& param_name, const int& exp_long);
};
#endif
# Frame names
odom_frame_id: odom
robot_frame_id: base_link
# Initial STD of Error-State values (filter)
dp0_std: [0.0, 0.0, 0.05]
dv0_std: [0.0, 0.0, 0.0]
......
......@@ -9,6 +9,7 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>px_comm</build_depend>
......@@ -16,6 +17,7 @@
<build_depend>tf</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>px_comm</run_depend>
......
......@@ -50,7 +50,7 @@ double EskfOdomAlgorithm::get_proc_time(void)
double proc_time = this->filter_.get_proc_time();
return proc_time;
}
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state)
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXd& state, Eigen::Vector3d& ang_vel)
{
return this->filter_.run_next_step(state);
return this->filter_.run_next_step(state,ang_vel);
}
\ No newline at end of file
#include "eskf_odom_alg_node.h"
using namespace std;
using namespace Eigen;
// STD stuff
#include <fstream>
#include <iostream>
#include <sstream>
// TF stuff
#include <tf/transform_broadcaster.h>
EskfOdomAlgNode::EskfOdomAlgNode(void) :
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
{
// Initialize all vars
this->loop_rate_ = 1000; //in [Hz]
this->seq_ = 0;
this->flying_ = false;
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();
this->loop_rate_ = 1000; //in [Hz]
// Initialize vars
this->is_first_imu_ = true;
this->is_first_px4_ = true;
this->new_imu_ = false;
this->new_px4_ = false;
this->tlast_imu_ = 0.0;
this->tlast_px4_ = 0.0;
// Read initial parameters from yaml file and set themn to sensors and filter
read_and_set_ini_params();
// [init publishers]
this->odom_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("odom", 1);
// [init subscribers]
this->px4_of_subscriber_ = this->public_node_handle_.subscribe("px4_of", 10,&EskfOdomAlgNode::px4_of_callback, this);
......@@ -53,6 +55,10 @@ EskfOdomAlgNode::~EskfOdomAlgNode(void)
void EskfOdomAlgNode::read_and_set_ini_params(void)
{
// Frame names
this->public_node_handle_.param<std::string>("odom_frame_id", this->odom_frame_id_, "odom");
this->public_node_handle_.param<std::string>("robot_frame_id", this->robot_frame_id_, "base_link");
// Initial STD Error-State values (filter parameters)
params f_params;
f_params.dp0_std = read_vec("dp0_std", 3);
......@@ -62,15 +68,14 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
f_params.dwb0_std = read_vec("dwb0_std", 3);
f_params.dg0_std = read_vec("dg0_std", 3);
string frame; // Frame r.t. orientation derivation is computed (g:global, l:local)
this->public_node_handle_.param<string>("frame", frame, "global");
std::string frame; // Frame r.t. orientation derivation is computed (g:global, l:local)
this->public_node_handle_.param<std::string>("frame", frame, "global");
if (frame == "global")
f_params.frame = 'g';
else if (frame == "local")
f_params.frame = 'l';
else
cout << "ERROR loading frame: Wrong frame name (should be local or global)"
<< endl;
std::cout << "ERROR loading frame: Wrong frame name (should be local or global)" << std::endl;
// IMU STD values
Sensor::imu_params imu_params;
......@@ -78,16 +83,14 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
imu_params.w_std = read_vec("imu_w_std", 3);
imu_params.ab_std = read_vec("imu_ab_std", 3);
imu_params.wb_std = read_vec("imu_wb_std", 3);
string imu_met;
this->public_node_handle_.param<string>("imu_method", imu_met, "continuous");
std::string imu_met;
this->public_node_handle_.param<std::string>("imu_method", imu_met, "continuous");
if (imu_met == "continuous")
imu_params.met = 'c';
else if (imu_met == "discrete")
imu_params.met = 'd';
else
cout
<< "ERROR loading frame: Wrong IMU method name (should be continuous or discrete)"
<< endl;
std::cout << "ERROR loading frame: Wrong IMU method name (should be continuous or discrete)" << std::endl;
// PX4 STD values
Sensor::px4_params px4_params;
......@@ -113,7 +116,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
this->alg_.unlock();
}
Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,const int& exp_long)
Eigen::VectorXd EskfOdomAlgNode::read_vec(const std::string& param_name,const int& exp_long)
{
Eigen::VectorXd params = Eigen::VectorXd::Zero(exp_long);
......@@ -129,17 +132,15 @@ Eigen::VectorXd EskfOdomAlgNode::read_vec(const string& param_name,const int& ex
params(ii) = my_list[ii];
}
else
cout << "ERROR loading " << param_name << ": Wrong elements number"
<< endl;
std::cout << "ERROR loading " << param_name << ": Wrong elements number" << std::endl;
}
else
cout << "ERROR loading " << param_name << ": Wrong element type" << endl;
std::cout << "ERROR loading " << param_name << ": Wrong element type" << std::endl;
return params;
}
void EskfOdomAlgNode::mainNodeThread(void)
{
// [fill msg structures]
// [fill srv structure and make request to the server]
......@@ -147,6 +148,7 @@ 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();
......@@ -156,10 +158,11 @@ void EskfOdomAlgNode::mainNodeThread(void)
this->alg_.unlock();
Eigen::VectorXd state(19,1);
Eigen::Vector3d ang_vel;
bool step_done = false;
this->alg_.lock();
step_done = this->alg_.run_next_step(state);
step_done = this->alg_.run_next_step(state,ang_vel);
this->alg_.unlock();
if (step_done)
......@@ -170,7 +173,28 @@ void EskfOdomAlgNode::mainNodeThread(void)
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(), "world", "base_link"));
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_;
};
}
......@@ -211,7 +235,6 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
this->is_first_imu_ = false;
}
// this->imu_queue_.push(msg);
set_imu_reading(msg, t-this->t_ini_imu_);
}
void EskfOdomAlgNode::imu_mutex_enter(void)
......
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