diff --git a/core_navigation/config/parameters.yaml b/core_navigation/config/parameters.yaml index b6e7153..9a0861f 100644 --- a/core_navigation/config/parameters.yaml +++ b/core_navigation/config/parameters.yaml @@ -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 diff --git a/core_navigation/include/core_navigation/CoreNav.h b/core_navigation/include/core_navigation/CoreNav.h index af0f80b..9030523 100644 --- a/core_navigation/include/core_navigation/CoreNav.h +++ b/core_navigation/include/core_navigation/CoreNav.h @@ -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; @@ -56,6 +57,7 @@ class CoreNav { CoreNav::Vector3 cmd; CoreNav::Vector2 encoderLeft; CoreNav::Vector2 encoderRight; + CoreNav::Vector cmd_stop; CoreNav(); ~CoreNav(); @@ -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(); @@ -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_; @@ -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_; @@ -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_; @@ -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_; @@ -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_; diff --git a/core_navigation/src/CoreNav.cpp b/core_navigation/src/CoreNav.cpp index d7873fb..27d8d49 100644 --- a/core_navigation/src/CoreNav.cpp +++ b/core_navigation/src/CoreNav.cpp @@ -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); @@ -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){ @@ -759,28 +688,29 @@ bool CoreNav::RegisterCallbacks(const ros::NodeHandle& n){ cn_pub_=nl.advertise("cn_odom", 10, false); slip_pub_=nl.advertise( "slip", 10, false); gp_pub=nl.advertise("gp_input", 10, false); - stop_cmd_pub_ = nl.advertise("/core_nav/core_nav/stop_cmd", 1); + // stop_cmd_pub_ = nl.advertise("/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]; @@ -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_); @@ -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"<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, @@ -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)< R_IP; Eigen::Matrix R_IP_1; Eigen::Matrix R_IP_2; Eigen::Matrix K_pred; Eigen::Matrix H_; + Eigen::Matrix P_pred; + Eigen::Matrix STM_; + Eigen::Matrix 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_; diff --git a/gp_predictor/src/gp_predictor.cpp b/gp_predictor/src/gp_predictor.cpp index a1494cd..202eee7 100644 --- a/gp_predictor/src/gp_predictor.cpp +++ b/gp_predictor/src/gp_predictor.cpp @@ -3,33 +3,27 @@ GpPredictor::GpPredictor(ros::NodeHandle &nh) :nh_(nh) { - - - // mobility_sub = nh_.subscribe("/state_machine/mobility_scout", 1, &GpPredictor::mobilityCallback, this); gp_sub_ =nh.subscribe("/core_nav/core_nav/gp_result",1, &GpPredictor::GPCallBack, this); clt_setStopping_ =nh_.serviceClient("/core_nav/core_nav/stopping_service"); stop_cmd_pub_ = nh.advertise("/core_nav/core_nav/stop_cmd", 1); - } void GpPredictor::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; + + std::cout << "size" << gp_data_.mean.size() <<'\n'; + ROS_INFO("New GP data is available for %.2f seconds", gp_data_.mean.size()/10.0); gp_arrived_time_ = ros::Time::now().toSec(); core_nav::SetStopping srv_set_stopping; - srv_set_stopping.request.stopping = new_gp_data_arrived_; + srv_set_stopping.request.stopping = true; if (clt_setStopping_.call(srv_set_stopping)) { + ROS_INFO("Called Stopping Service"); - savePos[0]=srv_set_stopping.response.PosData.x; - savePos[1]=srv_set_stopping.response.PosData.y; - savePos[2]=srv_set_stopping.response.PosData.z; - - // res.PosData=PosVec; for (int row=0; row<15; row++){ for (int col=0; col<15; col++){ P_pred(row,col)=srv_set_stopping.response.PvecData[row*15+col]; @@ -38,24 +32,31 @@ void GpPredictor::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_){ } } - for (int row1=0; row1<4; row1++){ for (int col1=0; col1<15; col1++){ H_(row1,col1)=srv_set_stopping.response.HvecData[row1*4+col1]; } } + savePos[0]=srv_set_stopping.response.PosData.x; + savePos[1]=srv_set_stopping.response.PosData.y; + savePos[2]=srv_set_stopping.response.PosData.z; + + ROS_INFO_STREAM("POS"< 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); + ROS_INFO("Stop Command Required, error is more than %.2f meters", xy_errSlip); + ROS_INFO("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) { @@ -110,12 +110,11 @@ void GpPredictor::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_){ 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); + ROS_INFO("Remaining Time to Stop = %.3f s", stop_cmd_msg_.data); break; // then break the for loop. } @@ -125,11 +124,11 @@ void GpPredictor::GPCallBack(const core_nav::GP_Output::ConstPtr& gp_data_in_){ 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... - 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); - gp_flag =false; // DONT FORGET THIS IN HERE + // 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... + // 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); + // gp_flag =false; // DONT FORGET THIS IN HERE } }