Commit d125cf91 authored by adrianamor's avatar adrianamor

Added node no-iri style to test callbacks and topic inputs in the near future...

Added node no-iri style to test callbacks and topic inputs in the near future (eskf_odom cpp and h).
parent ae9688d7
#ifndef _ESKF_ODOM_H
#define _ESKF_ODOM_H
#include "ros/ros.h"
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <Eigen/Dense>
// Subscribers
#include <sensor_msgs/Imu.h>
#include <px_comm/OpticalFlow.h>
#include <asctec_msgs/LLStatus.h>
// Algorithm (wrapper of the low-level library)
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "eskf_odom_alg.h"
using namespace std;
using namespace Eigen;
// class EskfOdom : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>
class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>
{
private:
vector <vector<double>> a_;
vector <vector<double>> w_;
vector <vector<double>> px4_;
vector <vector<double>> llstatus_;
vector <vector<double>> read_from_file(const string& path);
ros::NodeHandle public_node_handle_;
public:
// EskfOdom(ros::NodeHandle &nh);
EskfOdomAlgNode(ros::NodeHandle &nh);
// ~EskfOdom(void);
~EskfOdomAlgNode(void);
bool run();
};
#endif
#include "eskf_odom.h"
using namespace std;
// EskfOdom::EskfOdom(ros::NodeHandle &nh) :
EskfOdomAlgNode::EskfOdom(ros::NodeHandle &nh) :
public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>
{
this->public_node_handle_ = nh;
this->a_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/linear_acceleration.txt");
this->w_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/angular_velocity.txt");
this->px4_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/opt_flow.txt");
this->llstatus_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/ll_status.txt");
}
// EskfOdom::~EskfOdom(void)
EskfOdomAlgNode::~EskfOdom(void)
{
}
// vector <vector<double>> EskfOdom::read_from_file(const string& path)
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;
}
// bool EskfOdom::run()
bool EskfOdomAlgNode::run()
{
ros::Rate loop_rate(100);
int count = 0;
while (ros::ok())
{
ROS_DEBUG("time: %f",this->a_[count][0]);
ROS_DEBUG("ax: %f, ay: %f, az: %f", this->a_[count][1],this->a_[count][2],this->a_[count][3]);
ROS_DEBUG("wx: %f, wy: %f, wz: %f", this->w_[count][1],this->w_[count][2],this->w_[count][3]);
ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[count][4],this->px4_[count][7],this->px4_[count][8]);
ROS_DEBUG("flying: %f", this->llstatus_[count][10]);
this->alg_.lock(); // protect algorithm
this->alg_.unlock();
ros::spinOnce();
loop_rate.sleep();
++count;
}
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "eskf_odom");
ros::NodeHandle n;
// EskfOdom eskf_node(n);
EskfOdomAlgNode eskf_node(n);
eskf_node.run();
return EXIT_SUCCESS;
}
\ No newline at end of file
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