double Plane::getYaw(){
Eigen::Matrix<double,3,3> rotMat = getRotationMatrix();
Eigen::Vector3d vector = rotMat.eulerAngles(0,1,2);
if(vector[2]<-(M_PI*0.5)){
vector[2]+=M_PI;
}else if(vector[2]>(M_PI*0.5)){
vector[2]-=M_PI;
}
return vector[2];
}
void PositionCommand::getPose(const geometry_msgs::PoseStamped::ConstPtr & Pose)
{
rot_matrix_= Eigen::Quaterniond(Pose->pose.orientation.w,
Pose->pose.orientation.x,
Pose->pose.orientation.y,
Pose->pose.orientation.z).matrix();
Eigen::Matrix<double,3,1> euler = rot_matrix_.eulerAngles(2, 1, 0);
double yaw = euler(0,0);
double pitch = euler(1,0);
double roll = euler(2,0);
current_pose_ << Pose->pose.position.x,
Pose->pose.position.y,
Pose->pose.position.z,
roll,
pitch,
yaw;
// if it is the first time: initialize the function
if(init_)
{
ROS_INFO("initialize");
previous_time_ = ros::Time::now();
goal_pose_ << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
init_ = false;
control_ = false;
chirp_enable_ = false;
}
ros::Time current_time = Pose->header.stamp;
period_ = current_time.toSec() - previous_time_.toSec();
previous_time_ = current_time;
control_ = true;
control_info_msg.Period = period_;
control_info_msg.header.stamp = current_time;
control_info_pub.publish(control_info_msg);
// std::cout << "CP: x = " << current_pose_(0,0)
// << " y = " << current_pose_(1,0)
// << " z = " << current_pose_(2,0)
// << " w = " << current_pose_(5,0)
// << std::endl;
// std::cout << "Period =" << period_ << std::endl;
}
double Plane::getPitch(){
Eigen::Matrix<double,3,3> rotMat = getRotationMatrix();
Eigen::Vector3d vector = rotMat.eulerAngles(0,1,2);
return vector[0];
}