Commented callbacks to test into the robot

parent f22a2ebd
......@@ -51,7 +51,7 @@
using namespace std;
using namespace Eigen;
#define READ_FROM_FILE
//#define READ_FROM_FILE
/**
* \brief IRI ROS Specific Algorithm Class
......
......@@ -400,42 +400,44 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
}
void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
// Get sensor values
this->px4_of_mutex_enter();
this->gnd_dist_ = msg->ground_distance;
double vx = msg->velocity_x;
double vy = -msg->velocity_y; // axis change according to simulation
// ros::Time stamp = msg->header.stamp;
ros::Time stamp = ros::Time::now();
this->px4_of_mutex_exit();
Vector3d val;
val << this->gnd_dist_,vx,vy;
double t = stamp.toSec();
if (this->is_first_px4_){
this->t_ini_px4_= t;
this->is_first_px4_ = false;
}
double curr_t = t-this->t_ini_px4_;
double flowx = msg->flow_x;
double flowy = -msg->flow_y; // axis change according to simulation
Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
this->alg_.set_px4_reading(curr_t,val,flow); // Set values into filter object
this->alg_.unlock();
if (!this->is_first_imu_) // We first want to propagate with the IMU
this->new_px4_ = true;
// printf("PX4 %2.10f \n",t);
std::cout << "<<< PX4 at: " << msg->header.stamp.toSec()*1e-9 <<"\n";
// ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
//
// // Get sensor values
// this->px4_of_mutex_enter();
// this->gnd_dist_ = msg->ground_distance;
// double vx = msg->velocity_x;
// double vy = -msg->velocity_y; // axis change according to simulation
// // ros::Time stamp = msg->header.stamp;
// ros::Time stamp = ros::Time::now();
// this->px4_of_mutex_exit();
//
// Vector3d val;
// val << this->gnd_dist_,vx,vy;
// double t = stamp.toSec();
//
// if (this->is_first_px4_){
// this->t_ini_px4_= t;
// this->is_first_px4_ = false;
// }
//
// double curr_t = t-this->t_ini_px4_;
//
// double flowx = msg->flow_x;
// double flowy = -msg->flow_y; // axis change according to simulation
//
// Vector2d flow;
// flow << flowx,flowy;
//
// this->alg_.lock();
// this->alg_.set_px4_reading(curr_t,val,flow); // Set values into filter object
// this->alg_.unlock();
//
// if (!this->is_first_imu_) // We first want to propagate with the IMU
// this->new_px4_ = true;
//
// // printf("PX4 %2.10f \n",t);
}
......@@ -450,39 +452,41 @@ void EskfOdomAlgNode::hl_status_mutex_exit(void)
}
void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
// Get sensor values
this->imu_mutex_enter();
double ax = msg->linear_acceleration.x;
double ay = -msg->linear_acceleration.y; // axis change according to simulation
double az = -msg->linear_acceleration.z; // axis change according to simulation
double wx = msg->angular_velocity.x;
double wy = -msg->angular_velocity.y; // axis change according to simulation
double wz = -msg->angular_velocity.z;// axis change according to simulation
// ros::Time stamp = msg->header.stamp;
ros::Time stamp = ros::Time::now();
this->imu_mutex_exit();
double t = stamp.toSec();
if (this->is_first_imu_)
this->t_ini_imu_= t;
double curr_t = t-this->t_ini_imu_;
Vector3d a;
a << ax,ay,az;
Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
this->alg_.set_imu_reading(curr_t,a,w); // Set values into filter object
this->alg_.unlock();
this->new_imu_ = true;
// printf("IMU %2.10f \n",t);
std::cout << ">>> IMU at: " << msg->header.stamp.toSec()*1e-9 <<"\n";
// ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
//
// // Get sensor values
// this->imu_mutex_enter();
// double ax = msg->linear_acceleration.x;
// double ay = -msg->linear_acceleration.y; // axis change according to simulation
// double az = -msg->linear_acceleration.z; // axis change according to simulation
// double wx = msg->angular_velocity.x;
// double wy = -msg->angular_velocity.y; // axis change according to simulation
// double wz = -msg->angular_velocity.z;// axis change according to simulation
// // ros::Time stamp = msg->header.stamp;
// ros::Time stamp = ros::Time::now();
// this->imu_mutex_exit();
//
// double t = stamp.toSec();
//
// if (this->is_first_imu_)
// this->t_ini_imu_= t;
//
// double curr_t = t-this->t_ini_imu_;
//
// Vector3d a;
// a << ax,ay,az;
// Vector3d w;
// w << wx,wy,wz;
//
// this->alg_.lock();
// this->alg_.set_imu_reading(curr_t,a,w); // Set values into filter object
// this->alg_.unlock();
//
// this->new_imu_ = true;
//
// // printf("IMU %2.10f \n",t);
}
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