class PID { public: PID(double P,double I,double D) { this->Proportion = P; this->Derivative = D; this->Integral = I; } void PID_cale(double error); double PID_output; private: double Proportion; double Integral; double Derivative; double preErr; double sumErr; }; /******************** Define Variates **********************/ mavros_msgs::State current_state; mavros_msgs::BatteryStatus battery_state; sensor_msgs::MagneticField local_compass; geometry_msgs::TwistStamped Velocity_vector; mavros_msgs::SetMode offb_set_mode; geometry_msgs::PoseStamped local_pose_now; geometry_msgs::PoseStamped mix_pose; /********************** Flag **********************/ float degree; float line_degree; double error; double height; int off_flag; double roll, pitch, yaw; /********************** Function **********************/ double* cor_convert(double x, double y, float degree){ double *velo = new double[2]; velo[0] = x*cos(degree) + y*sin(degree); velo[1] = y*cos(degree) - x*sin(degree); return velo; } void state_cb1(const mavros_msgs::State::ConstPtr& msg) { current_state = *msg; } void Battery_get(const mavros_msgs::BatteryStatus::ConstPtr& msg) { battery_state = *msg; } void compass_get(const sensor_msgs::MagneticField msg) { local_compass = msg; degree = local_compass.magnetic_field.x*local_compass.magnetic_field_covariance[0]+ local_compass.magnetic_field.y*local_compass.magnetic_field_covariance[1]+ local_compass.magnetic_field.z*local_compass.magnetic_field_covariance[2]; } void local_pose_get(const geometry_msgs::PoseStamped pose1) { local_pose_now = pose1; height = local_pose_now.pose.position.z; tf::Quaternion q(local_pose_now.pose.orientation.x, local_pose_now.pose.orientation.y, local_pose_now.pose.orientation.z, local_pose_now.pose.orientation.w); tf::Matrix3x3 m(q); m.getRPY(roll, pitch, yaw); degree=180-yaw/3.1415326*180; } void get_cv( std_msgs::Int32MultiArray msg){ a[0] = msg.data[0]; a[1] = msg.data[1]; error = double(a[1])/100/20; line_degree = 90-atan(a[0])* 180 / 3.1415926; } int main(int argc, char *argv[]) { ros::init(argc, argv, "pid"); ros::NodeHandle nh; /********************** Subscriber **********************/ ros::Subscriber state_sub = nh.subscribe ("mavros/state", 10, state_cb1); ros::Subscriber battery_sub = nh.subscribe ("mavros/battery", 10, Battery_get); ros::Subscriber degree_local = nh.subscribe ("mavros/imu/mag", 10, compass_get); ros::Subscriber pid_error = nh.subscribe ("PID_ERROR", 5,error_get); ros::Subscriber cv_sub = nh.subscribe< std_msgs::Int32MultiArray> ("cv",10,get_cv); ros::Subscriber local_position_sub = nh.subscribe ("mavros/local_position/pose", 10, local_pose_get); /********************** Publisher **********************/ ros::Publisher setpoint_velocity = nh.advertise ("mavros/setpoint_velocity/cmd_vel", 10); ros::Publisher local_pos_pub = nh.advertise ("mavros/setpoint_position/local", 10); ros::Rate rate(20.0); /********************** Init Variates **********************/ offb_set_mode.request.custom_mode = "OFFBOARD"; Velocity_vector.twist.linear.x = 0; Velocity_vector.twist.linear.y = 0; Velocity_vector.twist.linear.z = 0; Velocity_vector.twist.angular.x = 0; Velocity_vector.twist.angular.y = 0; Velocity_vector.twist.angular.z = 0; PID black_line(P,I,D); ros::Time last_request = ros::Time::now(); while (ros::ok()) { if (current_state.mode == "OFFBOARD" && off_flag ==0) { mix_pose.pose.position.x = local_pose_now.pose.position.x; mix_pose.pose.position.y = local_pose_now.pose.position.y; mix_pose.pose.position.z = local_pose_now.pose.position.z; mix_pose.pose.orientation.x = 0; mix_pose.pose.orientation.y = 0; mix_pose.pose.orientation.z = sin(line_degree); mix_pose.pose.orientation.w = sin(line_degree); off_flag = 1; } else if(current_state.mode != "OFFBOARD") { mix_pose.pose.position.x = local_pose_now.pose.position.x; mix_pose.pose.position.y = local_pose_now.pose.position.y; mix_pose.pose.position.z = local_pose_now.pose.position.z; } black_line.PID_cale(error); double* velocity_array = cor_convert(0,black_line.PID_output,degree); Velocity_vector.twist.linear.y = velocity_array[1]; Velocity_vector.twist.linear.x = velocity_array[0]; setpoint_velocity.publish(Velocity_vector); //local_pos_pub.publish(mix_pose); ros::spinOnce(); rate.sleep(); } return 0; } void PID::PID_cale(double error) { double dErr; this->sumErr += error; if(this->sumErr>1) {this->sumErr = 1; } if(this->sumErr<-1) {this->sumErr = -1; } dErr = error - this->preErr; this->preErr = error; this->PID_output = this->Proportion*error +this->Derivative*dErr +this->Integral*this->sumErr; }