Skip to content

Commit

Permalink
add gp_predictor (tag release)
Browse files Browse the repository at this point in the history
  • Loading branch information
cagrikilic committed Oct 14, 2020
1 parent c10c4af commit 7ad47ff
Show file tree
Hide file tree
Showing 5 changed files with 96 additions and 138 deletions.
2 changes: 2 additions & 0 deletions core_navigation/config/parameters.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,8 @@ joint:
topic: /dead_reckoning/joint_states
gp:
topic: /core_nav/core_nav/gp_result
stop:
topic: /core_nav/core_nav/stop_cmd

frames:
frame_id_out: map
Expand Down
21 changes: 18 additions & 3 deletions core_navigation/include/core_navigation/CoreNav.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class CoreNav {
typedef nav_msgs::Odometry OdoData;
typedef sensor_msgs::JointState JointData;
typedef geometry_msgs::Twist CmdData;
// typedef std_msgs::Float64 StopData;
// typedef hw_interface_plugin_roboteq::RoboteqData EncoderLeftData;
// typedef hw_interface_plugin_roboteq::RoboteqData EncoderRightData;
typedef geometry_msgs::PoseStamped PoseData;
Expand All @@ -56,6 +57,7 @@ class CoreNav {
CoreNav::Vector3 cmd;
CoreNav::Vector2 encoderLeft;
CoreNav::Vector2 encoderRight;
CoreNav::Vector cmd_stop;

CoreNav();
~CoreNav();
Expand All @@ -80,10 +82,14 @@ class CoreNav {
void OdoCallback(const OdoData& odo_data_);
void JointCallBack(const JointData& joint_data_);
void CmdCallBack(const CmdData& cmd_data_);
// void stopCallback(const StopData& stop_data_);
// void EncoderLeftCallBack(const EncoderLeftData& encoderLeft_data_);
// void EncoderRightCallBack(const EncoderRightData& encoderRight_data_);

void GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_);
// void CoreNav::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_)
void stopCallback(const std_msgs::Float64::ConstPtr& msg);
// ros::Subscriber sub = nh.subscribe("/core_nav/core_nav/stop_cmd", 1, stopCallback);

// void Update(const CoreNav::Vector13& odo,const CoreNav::Vector4& joint);
void Update();
Expand Down Expand Up @@ -111,10 +117,13 @@ class CoreNav {
CoreNav::Vector6 getImuData(const ImuData& imu_data_);
CoreNav::Vector4 getJointData(const JointData &joint_data_);
CoreNav::Vector getCmdData(const CmdData &cmd_data_);
// std_msgs::Float64 getStopData(const StopData& stop_data_);
// CoreNav::Vector2 getEncoderLeftData(const EncoderLeftData &encoderLeft_data_);
// CoreNav::Vector2 getEncoderRightData(const EncoderRightData &encoderRight_data_);
CoreNav::Vector13 getOdoData(const OdoData& odo_data_);



// The node's name.
std::string name_;

Expand All @@ -126,7 +135,9 @@ class CoreNav {
ros::Subscriber encoderLeft_sub_;
ros::Subscriber encoderRight_sub_;
ros::Subscriber gp_sub_;
bool new_gp_data_arrived_;
ros::Subscriber stop_sub_;

bool new_stop_data_arrived_;
double gp_arrived_time_;
// Publisher.
ros::Publisher position_pub_, velocity_pub_, attitude_pub_, enu_pub_,cn_pub_, slip_pub_, gp_pub, stop_cmd_pub_,pos_llh_pub_,ins_xyz_pub_;
Expand All @@ -137,6 +148,8 @@ class CoreNav {
ImuData imu_data_;
JointData joint_data_;
CmdData cmd_data_;
// StopData stop_data_;

// EncoderLeftData encoderLeft_data_;
// EncoderRightData encoderRight_data_;
std_msgs::Float64 stop_cmd_msg_;
Expand Down Expand Up @@ -177,7 +190,9 @@ class CoreNav {
std::string cmd_topic_;
std::string encoderLeft_topic_;
std::string encoderRight_topic_;
std::string gp_topic_; // CAGRI: this is fine, you just need to set it with a param, like you do with the rest of these. It isn't being set to anything yet in the .cpp file
std::string gp_topic_;
std::string stop_topic_;
// CAGRI: this is fine, you just need to set it with a param, like you do with the rest of these. It isn't being set to anything yet in the .cpp file
// For initialization.
bool initialized_;

Expand Down Expand Up @@ -220,7 +235,7 @@ class CoreNav {
CoreNav::Vector3 H11_, H12_, H21_, H31_, H32_, H24_, H41_, H42_;
double z11_, z21_, z31_, z41_;
double rearVel_, headRate_, T_r_, s_or_, s_delta_or_,velFrontLeft_,velFrontRight_,velBackLeft_,velBackRight_;

double cmd_stop_;
CoreNav::Vector3 omega_b_ib_, omega_b_ib_prev_, omega_n_ie_;
CoreNav::Vector3 f_ib_b_, f_ib_b_prev_, omega_n_en_, omega_n_in_, grav_;
CoreNav::Matrix3 Omega_b_ib_, Omega_n_ie_, Omega_n_en_;
Expand Down
142 changes: 38 additions & 104 deletions core_navigation/src/CoreNav.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -318,85 +318,12 @@ if (slip !=0.0 && slip !=-1.0 && slip !=1.0 && fabs(cmd[0])>0.2) //&& cmd_vel ==
slip_msg.time_array.clear();
}

if(new_gp_data_arrived_) // This will happen after Gaussian Process publishes its results, check line 726. It initialized as false.
if(new_stop_data_arrived_)
{
ROS_DEBUG("NEW GP DATA ARRIVED!");

double cmd_stop_ = 0.0;

// gp_predictor_service(STM_,P_pred,Q_,H_,savePos);
//line 1100 R_IP_1 should be defined in gp_predictor
//K_pred should be defined in gp_predictor

// for(int slip_i=0; slip_i<5*gp_data_.mean.size(); slip_i++) // HERE
// {
// P_pred = STM_*P_pred*STM_.transpose() + Q_;
// if ((slip_i % 5) ==0) //odomdata available //HERE
// {
// double chi0_slip=gp_data_.mean.at(i);
// double chi1_slip=gp_data_.mean.at(i)+gp_data_.sigma.at(i);
// double chi2_slip=gp_data_.mean.at(i)-gp_data_.sigma.at(i);
//
// double chi0_odo=0.8/(1.0-chi0_slip);
// double chi1_odo=0.8/(1.0-chi1_slip);
// double chi2_odo=0.8/(1.0-chi2_slip);
//
// double chi_UT_est=(chi0_odo+chi1_odo+chi2_odo)/3.0;
// double chi_UT_est_cov=((chi0_odo-chi_UT_est)*(chi0_odo-chi_UT_est)+(chi1_odo-chi_UT_est)*(chi1_odo-chi_UT_est)+(chi2_odo-chi_UT_est)*(chi2_odo-chi_UT_est))/3.0;
//
// R_IP_2<< std::max(0.03*0.03,chi_UT_est_cov*chi_UT_est_cov),0,0,0,
// 0,std::max(0.03*0.03,chi_UT_est_cov*chi_UT_est_cov),0,0,
// 0,0,std::max(0.05*0.05,chi_UT_est_cov*chi_UT_est_cov),0,
// 0,0,0,0.05*0.05;
//
// R_IP=25*R_IP_1*R_IP_2*R_IP_1.transpose();
// // R_IP=R_IP_1*R_IP_2*R_IP_1.transpose();
//
//
// K_pred=P_pred*H_.transpose()*(H_*P_pred*H_.transpose() +R_IP).inverse();
// P_pred=(Eigen::MatrixXd::Identity(15,15) - K_pred*H_)*P_pred*(Eigen::MatrixXd::Identity(15,15)-K_pred*H_).transpose() + K_pred*R_IP*K_pred.transpose();
// // std::cout << "i:" << i << '\n';
// i++;
// }
//
// ins_enu_slip << CoreNav::llh_to_enu(savePos[0],savePos[1],savePos[2]);
// ins_enu_slip_3p << CoreNav::llh_to_enu(savePos[0]-3.0*sqrt(std::abs(P_pred(6,6))),savePos[1]-3.0*sqrt(std::abs(P_pred(7,7))), savePos[2]-3.0*sqrt(std::abs(P_pred(8,8))));
// ins_enu_slip3p << CoreNav::llh_to_enu(savePos[0]+3.0*sqrt(std::abs(P_pred(6,6))),savePos[1]+3.0*sqrt(std::abs(P_pred(7,7))), savePos[2]+3.0*sqrt(std::abs(P_pred(8,8))));
//
// xy_errSlip = sqrt((ins_enu_slip3p(0)-ins_enu_slip(0))*(ins_enu_slip3p(0)-ins_enu_slip(0)) + (ins_enu_slip3p(1)-ins_enu_slip(1))*(ins_enu_slip3p(1)-ins_enu_slip(1)));
// ROS_ERROR_THROTTLE(0.5,"XYerror %.6f meters", xy_errSlip);
// // std::cout << "error" << '\n'<< xy_errSlip << '\n';
//
// if (xy_errSlip > 2.00)
// {
//
// ROS_ERROR("Stop Command Required, error is more than %.2f meters", xy_errSlip);
// ROS_ERROR("Stop command should be set at %u seconds after %.2f sec driving",i/10,odomUptCount/10.0);
// if (gp_arrived_time_ + i/10.0 - ros::Time::now().toSec()<0.0) // if the results from GP arrival time and the time for each odometry update sum is less then the current time stop immediately. This means delta time is negative--we needed to stop earlier.
// {
// // if (gp_arrived_time_ + i/10.0 <0.0) {
// stop_cmd_msg_.data = 0.5;
// cmd_stop_=stop_cmd_msg_.data;
// }
// else //otherwise calculate the necessary time for stopping -- when do we need to stop from now.
// {
// stop_cmd_msg_.data = gp_arrived_time_ + i/10.0 - ros::Time::now().toSec();
// // stop_cmd_msg_.data = gp_arrived_time_ + i/10.0 ;
// cmd_stop_=stop_cmd_msg_.data;
//
// }
// stop_cmd_pub_.publish(stop_cmd_msg_); //publish the delta time required to stop.
// ROS_DEBUG("Remaining Time to Stop = %.3f s", stop_cmd_msg_.data);
//
// break; // then break the for loop.
// }
//
// }
ROS_DEBUG("NEW STOP DATA ARRIVED!");

new_gp_data_arrived_ = false; // Set the flag back to false, so that this does not happen again until new data comes in on the subscriber callback and sets this flag back to true
i=0.0;
slip_i=0.0;
startRecording=stopRecording+ceil(cmd_stop_)*10+10+30; // Do we need a buffer time? Since we are stopping 3 seconds, and there could be some other time added to the process...idk...
new_stop_data_arrived_ = false; // Set the flag back to false, so that this does not happen again until new data comes in on the subscriber callback and sets this flag back to true
startRecording=stopRecording+ceil(cmd_stop_)*10+10+30;
stopRecording=startRecording+150; //It should be 150...
ROS_WARN("Start Next Recording at %.2f", startRecording/10);
ROS_WARN("Stop Next Recording at %.2f", stopRecording/10);
Expand All @@ -423,6 +350,8 @@ if (slip !=0.0 && slip !=-1.0 && slip !=1.0 && fabs(cmd[0])>0.2) //&& cmd_vel ==
PublishStatesSlip(slip_cn_, slip_pub_);
return;
}


// PSEUDO MEASUREMENT UPDATES
void CoreNav::NonHolonomic(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 error_states, Eigen::MatrixXd P, CoreNav::Vector3 omega_b_ib){

Expand Down Expand Up @@ -759,28 +688,29 @@ bool CoreNav::RegisterCallbacks(const ros::NodeHandle& n){
cn_pub_=nl.advertise<nav_msgs::Odometry>("cn_odom", 10, false);
slip_pub_=nl.advertise<geometry_msgs::PointStamped>( "slip", 10, false);
gp_pub=nl.advertise<core_nav::GP_Input>("gp_input", 10, false);
stop_cmd_pub_ = nl.advertise<std_msgs::Float64>("/core_nav/core_nav/stop_cmd", 1);
// stop_cmd_pub_ = nl.advertise<std_msgs::Float64>("/core_nav/core_nav/stop_cmd", 1);


stop_sub_= nl.subscribe(stop_topic_, 1, &CoreNav::stopCallback, this);
imu_sub_ = nl.subscribe(imu_topic_, 1, &CoreNav::ImuCallback, this);
odo_sub_ = nl.subscribe(odo_topic_, 1, &CoreNav::OdoCallback, this);
joint_sub_ = nl.subscribe(joint_topic_, 1, &CoreNav::JointCallBack, this);
cmd_sub_ = nl.subscribe(cmd_topic_, 1, &CoreNav::CmdCallBack, this);
// encoderLeft_sub_ = nl.subscribe(encoderLeft_topic_, 1, &CoreNav::EncoderLeftCallBack, this);
// encoderRight_sub_ = nl.subscribe(encoderRight_topic_, 1, &CoreNav::EncoderRightCallBack, this);
gp_sub_ =nl.subscribe(gp_topic_,1, &CoreNav::GPCallBack, this);
// gp_sub_ =nl.subscribe(gp_topic_,1, &CoreNav::GPCallBack, this);

setStoppingService_ = nl.advertiseService("stopping_service",&CoreNav::setStopping_, this);

return true;
}



bool CoreNav::setStopping_(core_nav::SetStopping::Request &req, core_nav::SetStopping::Response &res){
ROS_DEBUG(" GOT SOME MESSAGE: %d", req.stopping);
ROS_DEBUG(" Message request!: %d", req.stopping);
flag_stopping = req.stopping;
// for (int i=0; i<225; i++)
// {
// res.PvecData[i] = Pvec[i];
// }

PosVec.x=savePos[0];
PosVec.y=savePos[1];
PosVec.z=savePos[2];
Expand All @@ -799,23 +729,13 @@ bool CoreNav::setStopping_(core_nav::SetStopping::Request &req, core_nav::SetSto
res.HvecData[row1*4+col1]=H_(row1,col1);
}
}
// res.PvecData.data() << Pvec;
// res.QvecData.data() << Qvec;
// res.STMvecData.data() << STMvec;

// float Pvecfloat[225];
// float Qvecfloat[225];
// float STMvecfloat[225];
// std::copy(Pvec,Pvec+225, Pvecfloat);
// std::copy(Qvec, Qvecfloat);
// std::copy(STMvec, STMvecfloat);

// res.Qvec = Qvecfloat;
// res.STMvec = STMvecfloat;

return true;
}


// void stopCallback(const std_msgs::Float64::ConstPtr& msg);

void CoreNav::OdoCallback(const OdoData& odo_data_){

odo = getOdoData(odo_data_);
Expand Down Expand Up @@ -889,14 +809,23 @@ void CoreNav::CmdCallBack(const CmdData& cmd_data_){
has_cmd_ = true;
return;
}
void CoreNav::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_){
this->gp_data_.mean = gp_data_in_->mean;
this->gp_data_.sigma = gp_data_in_->sigma;
new_gp_data_arrived_ = true;
ROS_INFO("New GP data is available for %.2f seconds", gp_data_.mean.size()/10.0);
gp_arrived_time_ = ros::Time::now().toSec();


// void CoreNav::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_){
// this->gp_data_.mean = gp_data_in_->mean;
// this->gp_data_.sigma = gp_data_in_->sigma;
// new_gp_data_arrived_ = true;
// ROS_INFO("New GP data is available for %.2f seconds", gp_data_.mean.size()/10.0);
// gp_arrived_time_ = ros::Time::now().toSec();
//
// }
void CoreNav::stopCallback(const std_msgs::Float64::ConstPtr& msg){
this ->cmd_stop_=msg->data;
ROS_ERROR_STREAM("cmd_stop"<<cmd_stop_);
ROS_ERROR_STREAM("msg.data"<<msg->data);
new_stop_data_arrived_=true;
}

CoreNav::Vector13 CoreNav::getOdoData(const OdoData& odo_data_){

CoreNav::Vector13 odoVec( (Vector(13) << odo_data_.pose.pose.position.x,
Expand Down Expand Up @@ -954,6 +883,10 @@ CoreNav::Vector CoreNav::getCmdData(const CmdData& cmd_data_){
}
return cmdVec;
}
// CoreNav::Vector CoreNav::getStopData(const StopData& stop_data_){
// CoreNav::Vector stopTime((Vector(1)<<stop_data_.data).finished());
// return stopTime;
// }

// PUBLISHERS
void CoreNav::PublishStates(const CoreNav::Vector3& states,const ros::Publisher& pub){
Expand Down Expand Up @@ -1028,6 +961,7 @@ bool CoreNav::LoadParameters(const ros::NodeHandle& n){
if(!pu::Get("odo/topic", odo_topic_)) return false;
if(!pu::Get("joint/topic", joint_topic_)) return false;
if(!pu::Get("cmd/topic", cmd_topic_)) return false;
if(!pu::Get("stop/topic", stop_topic_)) return false;
if(!pu::Get("encoderLeft/topic", encoderLeft_topic_)) return false;
if(!pu::Get("encoderRight/topic", encoderLeft_topic_)) return false;
if(!pu::Get("gp/topic", gp_topic_)) return false;
Expand Down Expand Up @@ -1104,7 +1038,7 @@ bool CoreNav::Initialize(const ros::NodeHandle& n){
ROS_ERROR("%s: Failed to register callbacks.", name_.c_str());
return false;
}
new_gp_data_arrived_ = false;
new_stop_data_arrived_ = false;

return true;
}
Expand Down
12 changes: 10 additions & 2 deletions gp_predictor/include/gp_predictor/gp_predictor.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
class GpPredictor
{
public:

GpPredictor(ros::NodeHandle &);
void mobility(bool flag);
void mobilityCallback(const std_msgs::Int64::ConstPtr& msg);
Expand All @@ -30,13 +31,20 @@ class GpPredictor
bool new_gp_data_arrived_;
double gp_arrived_time_;
typedef Eigen::MatrixXd Matrix;
GpPredictor::Matrix P_pred, STM_, Q_;
int slip_i, i;
// GpPredictor::Matrix P_pred, STM_, Q_;
int slip_i=0;
int i=0;;
Eigen::Matrix<double, 4, 4> R_IP;
Eigen::Matrix<double, 4, 4> R_IP_1;
Eigen::Matrix<double, 4, 4> R_IP_2;
Eigen::Matrix<double, 15, 4> K_pred;
Eigen::Matrix<double, 4, 15> H_;
Eigen::Matrix<double, 15, 15> P_pred;
Eigen::Matrix<double, 15, 15> STM_;
Eigen::Matrix<double, 15, 15> Q_;



GpPredictor::Vector3 savePos, ins_enu_slip, ins_enu_slip3p, ins_enu_slip_3p;
double xy_errSlip,odomUptCount, startRecording, stopRecording, saveCountOdom;
std_msgs::Float64 stop_cmd_msg_;
Expand Down
Loading

0 comments on commit 7ad47ff

Please sign in to comment.