diff --git a/CMakeLists.txt b/CMakeLists.txt index 50ff486..7fa04c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation message_runtime roscpp - controller_msgs ) ## System dependencies are found with CMake's conventions @@ -77,7 +76,6 @@ generate_messages( std_msgs geometry_msgs sensor_msgs - controller_msgs ) ################################################ diff --git a/config/noCtrl_param.yaml b/config/noCtrl_param.yaml index 92b4b98..c317ad3 100644 --- a/config/noCtrl_param.yaml +++ b/config/noCtrl_param.yaml @@ -1,4 +1,4 @@ # Ros param when not using Klausen Ctrl wait: 55 -waypoints: {x: 0.0, y: 0.0, z: 3.0} +waypoints: {x: 0.0, y: 0.0, z: 5.0} diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch new file mode 100644 index 0000000..07c0af2 --- /dev/null +++ b/launch/oscillation_damp.launch @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/takeoff_noCtrl.launch b/launch/takeoff_noCtrl.launch index 56ecfe7..865d612 100644 --- a/launch/takeoff_noCtrl.launch +++ b/launch/takeoff_noCtrl.launch @@ -1,10 +1,13 @@ - - + + + + + + + + + + diff --git a/msg/RefSignal.msg b/msg/RefSignal.msg index 65253d4..415a70b 100644 --- a/msg/RefSignal.msg +++ b/msg/RefSignal.msg @@ -1,7 +1,7 @@ # Contains array of smooth path signal at a given time # ref sign contains des pos, vel, and acc # -#std_msgs/Header header # Timestamp +std_msgs/Header header # Timestamp geometry_msgs/Vector3 position # Desired pos geometry_msgs/Vector3 velocity # Desired vel geometry_msgs/Vector3 acceleration # Desired acc diff --git a/src/LinkState.py b/src/LinkState.py index 040e4c1..bc97ef5 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -207,7 +207,9 @@ class Main: # Print and save results #print "\n" rospy.loginfo("") - print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(self.drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(self.drone_Eul[2]*180/3.14,2)) + print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)) + print"Pitch: " +str(round(self.drone_Eul[1]*180/3.14,2)) + print"Yaw: " +str(round(self.drone_Eul[2]*180/3.14,2)) print "drone pos.x: " + str(round(self.drone_Px,2)) print "drone pos.y: " + str(round(self.drone_Py,2)) print "drone pos.z: " + str(round(self.drone_Pz,2)) @@ -231,4 +233,4 @@ if __name__=="__main__": rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: - pass \ No newline at end of file + pass diff --git a/src/klausen_control.py b/src/klausen_control.py index 38c7c85..376c18b 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -14,13 +14,11 @@ import math from tf.transformations import * from scipy.integrate import odeint -from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles -from controller_msgs.msg import FlatTarget -from geometry_msgs.msg import Point, Pose -from geometry_msgs.msg import TwistStamped, Vector3, Vector3Stamped, PoseStamped, Quaternion -from gazebo_msgs.srv import GetLinkState +from oscillation_ctrl.msg import RefSignal, LoadAngles +from oscillation_ctrl.srv import WaypointTrack +from geometry_msgs.msg import Pose, Point, TwistStamped, PoseStamped from sensor_msgs.msg import Imu -from pymavlink import mavutil +from mavros_msgs.msg import AttitudeTarget class Main: @@ -42,20 +40,24 @@ class Main: # Msgs types self.vel_data = TwistStamped() # This is needed to get drone vel from gps - self.imu_data = Imu() # Needed for to get drone acc from IMU + self.imu_data = Imu() # Needed for to get drone acc from IMU self.path_pos = np.zeros([3,1]) - self.path_vel = np.zeros([3,1]) + self.path_vel = np.zeros([3,1]) self.path_acc = np.zeros([3,1]) self.dr_pos = Pose() - self.quaternion = PoseStamped() # used to send quaternion attitude commands + self.att_targ = AttitudeTarget() # used to send quaternion attitude commands self.load_angles = LoadAngles() self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q + # Service var + self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) + self.empty_point = Point() # Needed to query waypoint_server + # Drone var self.has_run = 0 # Bool to keep track of first run instance # Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI - self.PHI = np.array([[0,0],[0,0]]) + self.PHI = np.array([[0,0],[0,0]]) self.dr_vel = np.zeros([3,1]) self.dr_angVel = np.zeros([3,1]) self.dr_acc = np.zeros([3,1]) #self.imu_data.linear_acceleration @@ -93,33 +95,41 @@ class Main: self.drone_m = 1.437 self.pl_m = 0.5 self.tot_m = self.drone_m + self.pl_m + self.tune = 1 # Tuning parameter self.dist = np.array([0,0,0,0.01,0.01]) # Wind disturbance + # PD Thrust Controller terms + # gains for thrust PD Controller + self.Kp = 2.7 + self.Kd = 3 #2 + self.max_a = 14.2 + self.max_t = self.drone_m*self.max_a + self.R = np.empty([3,3]) # rotation matrix + self.e3 = np.array([[0],[0],[1]]) + self.thrust_offset = 0.7 # There was found to be a constant offset # --------------------------------------------------------------------------------# -# SUBSCRIBERS +# SUBSCRIBERS # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) - rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb) + rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb) #rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) # --------------------------------------------------------------------------------# -# PUBLISHERS +# PUBLISHERS # --------------------------------------------------------------------------------# - self.pub_q = rospy.Publisher('/command/quaternions',PoseStamped,queue_size = 10) + self.pub_att_targ = rospy.Publisher('/command/att_target',AttitudeTarget,queue_size = 10) # timer(s), used to control method loop freq(s) as defined by the rate(s) self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before - -# ------------------------------------ _init_ ends ------------------------------ # # --------------------------------------------------------------------------------# -# CALLBACK FUNCTIONS +# CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# # Callback pload deflection angles and vel @@ -137,7 +147,7 @@ class Main: # Callback drone pose def dronePos_cb(self,msg): try: - self.dr_pos = msg.pose + self.dr_pos = msg.pose #self.dr_pos = msg.drone_pos except ValueError: @@ -177,6 +187,13 @@ class Main: else: rospy.loginfo_once('NO TETHER DETECTED') + def waypoints_srv_cb(self): + rospy.wait_for_service('/status/waypoint_tracker') + try: + resp = self.get_xd(False,self.empty_point) + self.xd = resp.xd + except ValueError: + pass # ---------------------------------ODE SOLVER-------------------------------------# def statespace(self,y,t,Ka,Kb,Kc): @@ -190,8 +207,66 @@ class Main: return dydt # --------------------------------------------------------------------------------# + def quaternion_rotation_matrix(self): + """ + Covert a quaternion into a full three-dimensional rotation matrix. - def control(self): + Input + :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) + + Output + :return: A 3x3 element matrix representing the full 3D rotation matrix. + This rotation matrix converts a point in the local reference + frame to a point in the global reference frame. + """ + # Extract the values from Q + q0 = self.dr_pos.orientation.w + q1 = self.dr_pos.orientation.x + q2 = self.dr_pos.orientation.y + q3 = self.dr_pos.orientation.z + + # First row of the rotation matrix + r00 = 2 * (q0 * q0 + q1 * q1) - 1 + r01 = 2 * (q1 * q2 - q0 * q3) + r02 = 2 * (q1 * q3 + q0 * q2) + + # Second row of the rotation matrix + r10 = 2 * (q1 * q2 + q0 * q3) + r11 = 2 * (q0 * q0 + q2 * q2) - 1 + r12 = 2 * (q2 * q3 - q0 * q1) + + # Third row of the rotation matrix + r20 = 2 * (q1 * q3 - q0 * q2) + r21 = 2 * (q2 * q3 + q0 * q1) + r22 = 2 * (q0 * q0 + q3 * q3) - 1 + + # 3x3 rotation matrix + rot_matrix = np.array([[r00, r01, r02], + [r10, r11, r12], + [r20, r21, r22]]) + + return rot_matrix + + def determine_throttle(self): + # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) + # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch + self.waypoints_srv_cb() + self.error = np.array([[self.dr_pos.position.x - self.xd.x], + [self.dr_pos.position.y - self.xd.y], + [self.dr_pos.position.z - self.xd.z - self.thrust_offset]]) + + self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix + + + #thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t + thrust_vector = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel.T,self.R.T[2])/self.max_t + thrust = thrust_vector[2] + + # Value needs to be between 0 - 1.0 + self.att_targ.thrust = max(0.0,min(thrust,1.0)) + + + def determine_q(self): # Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi] p = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]]) g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]]) @@ -305,17 +380,19 @@ class Main: q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) # Populate msg variable - # Attitude control - self.quaternion.header.stamp = rospy.Time.now() - self.quaternion.pose.orientation.x = q[0] - self.quaternion.pose.orientation.y = q[1] - self.quaternion.pose.orientation.z = q[2] - self.quaternion.pose.orientation.w = q[3] + self.att_targ.header.stamp = rospy.Time.now() + self.att_targ.header.frame_id = 'map' + self.att_targ.type_mask = 1|2|4 + self.att_targ.orientation.x = q[0] + self.att_targ.orientation.y = q[1] + self.att_targ.orientation.z = q[2] + self.att_targ.orientation.w = q[3] def publisher(self,pub_time): - self.control() - self.pub_q.publish(self.quaternion) + self.determine_q() + self.determine_throttle() + self.pub_att_targ.publish(self.att_targ) #rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1]) # --------------------------------------------------------------------------------# @@ -328,7 +405,7 @@ if __name__=="__main__": # Initiate ROS node rospy.init_node('trajectoryCtrl',anonymous=True) try: - Main() # create class object + Main() # create class object rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: diff --git a/src/offb_node.cpp b/src/offb_node.cpp index 28e69bc..b43710b 100644 --- a/src/offb_node.cpp +++ b/src/offb_node.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -56,12 +57,6 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ } -// Determine new waypoins -geometry_msgs::PoseStamped wpoints; -bool des_waypoints = true; -void waypoints_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ - wpoints = *msg; -} int main(int argc, char **argv) { @@ -78,17 +73,21 @@ int main(int argc, char **argv) ("mavros/local_position/pose",10,mavPose_cb); // Waypoint Subscriber + /* ros::Subscriber waypoint_sub = nh.subscribe ("/reference/waypoints",10,waypoints_cb); - + */ + ros::ServiceClient waypoint_cl = nh.serviceClient + ("status/waypoint_tracker"); + /********************** PUBLISHERS **********************/ // Initiate publisher to publish commanded local position ros::Publisher local_pos_pub = nh.advertise - ("mavros/setpoint_position/local", 10); + ("mavros/setpoint_position/local", 10); // Publish desired attitude ros::Publisher thrust_pub = nh.advertise - ("mavros/setpoint_attitude/thrust", 10); + ("mavros/setpoint_attitude/thrust", 10); // Publish attitude commands ros::Publisher att_pub = nh.advertise @@ -96,9 +95,9 @@ int main(int argc, char **argv) // Service Clients ros::ServiceClient arming_client = nh.serviceClient - ("mavros/cmd/arming"); + ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient - ("mavros/set_mode"); + ("mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient ("mavros/cmd/takeoff"); @@ -110,16 +109,26 @@ int main(int argc, char **argv) ros::spinOnce(); rate.sleep(); } - - // Populate pose msg - pose.pose.position.x = 0; - pose.pose.position.y = 0; - pose.pose.position.z = 1.5; + + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)) + { + ROS_INFO("Waypoints received"); + } + + // populate desired waypoints + pose.pose.position.x = wpoint_srv.response.xd.x; + pose.pose.position.y = wpoint_srv.response.xd.y; + pose.pose.position.z = wpoint_srv.response.xd.z; + + /*/ Populate pose msg pose.pose.orientation.x = q_init.x; pose.pose.orientation.y = q_init.y; pose.pose.orientation.z = q_init.z; pose.pose.orientation.w = q_init.w; - + */ //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i){ @@ -154,10 +163,12 @@ int main(int argc, char **argv) last_request = ros::Time::now(); } } - if(des_waypoints == true){ - pose.pose.position.x = wpoints.pose.position.x; - pose.pose.position.y = wpoints.pose.position.y; - } + // Update desired waypoints + waypoint_cl.call(wpoint_srv); + pose.pose.position.x = wpoint_srv.response.xd.x; + pose.pose.position.y = wpoint_srv.response.xd.y; + pose.pose.position.z = wpoint_srv.response.xd.z; + local_pos_pub.publish(pose); diff --git a/src/path_follow.cpp b/src/path_follow.cpp index f5903ab..cd11285 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -10,54 +10,37 @@ #include #include #include -#include #include #include #include #include #include -#include -#include #include #include #include #include // for std::abs /********* CALLBACK FUNCTIONS **********************/ -// Initiate variables -mavros_msgs::State current_state; - -geometry_msgs::PoseStamped desPose; - -tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent; -double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = Klausen -geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg -oscillation_ctrl::EulerAngles eulAng; - -mavros_msgs::AttitudeTarget thrust; // Callback function which will save the current state of the autopilot. // Allows to check connection, arming, and Offboard tags*/ +mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } -geometry_msgs::Quaternion q_des; -// Cb to determine desired pose *Only needed for orientation -void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ - desPose = *msg; - q_des = desPose.pose.orientation; - tf2::convert(q_des,q_cmd); // Convert msg type to tf2::quaternion type +// Cb to determine attitude target +mavros_msgs::AttitudeTarget att_targ; +void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){ + att_targ = *msg; } - // Cb to recieve pose information geometry_msgs::PoseStamped buff_pose; geometry_msgs::Quaternion q_init; geometry_msgs::PoseStamped mavPose; bool gps_read = false; double current_altitude; - void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ mavPose = *msg; current_altitude = mavPose.pose.position.z; @@ -75,12 +58,6 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ } } -// To determine throttle ... type -void thrust_cb(const mavros_msgs::AttitudeTarget msg){ - thrust = msg; - tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat -} - int main(int argc, char **argv) { ros::init(argc, argv, "path_follow"); @@ -91,31 +68,23 @@ int main(int argc, char **argv) ros::Subscriber state_sub = nh.subscribe ("mavros/state", 10, state_cb); - // Get quaternions from klausen control - ros::Subscriber q_sub = nh.subscribe - ("command/quaternions",10,q_cb); + // Get attitude target from klausen control + ros::Subscriber att_target_sub = nh.subscribe + ("command/att_target",10,att_targ_cb); // Pose subscriber ros::Subscriber mavPose_sub = nh.subscribe ("mavros/local_position/pose",10,mavPose_cb); - // Throttle: Jaeyoung-Lim - ros::Subscriber thro_sub = nh.subscribe - ("command/bodyrate_command",10,thrust_cb); - /********************** PUBLISHERS **********************/ // Initiate publisher to publish commanded local position ros::Publisher local_pos_pub = nh.advertise ("mavros/setpoint_position/local", 10); // Publish attitude and thrust commands - ros::Publisher att_targ = nh.advertise + ros::Publisher att_targ_pub = nh.advertise ("mavros/setpoint_raw/attitude",10); - // Publish attitude cmds in euler angles - ros::Publisher euler_pub = nh.advertise - ("command/euler_angles",10); - // Service Clients ros::ServiceClient arming_client = nh.serviceClient ("mavros/cmd/arming"); @@ -145,11 +114,6 @@ int main(int argc, char **argv) /*********** Initiate variables ************************/ //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms ros::Rate rate_pub(75.0); - // Needed for attitude command - mavros_msgs::AttitudeTarget attitude; - - attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO - attitude.type_mask = 1|2|4; // Ignore body rates // Retrieve desired waypoints oscillation_ctrl::WaypointTrack wpoint_srv; @@ -191,69 +155,46 @@ int main(int argc, char **argv) while(ros::ok()){ if(gps_read == true){ - // Now need to convert publish q to euler ang - tf2::Matrix3x3 m_K(q_cmd); tf2::Matrix3x3 m_J(q_Jae); - m_K.getRPY(rol_K, pch_K, yaw_K); - m_J.getRPY(rol_J, pch_J, yaw_J); - - // Populate msg - K_ang.x = 180*rol_K/3.141; - K_ang.y = 180*pch_K/3.141; - K_ang.z = yaw_K; - - J_ang.x = rol_J; - J_ang.y = pch_J; - J_ang.z = yaw_J; - - eulAng.commanded = K_ang; - eulAng.desired = J_ang; - - if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ - if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ - } else { - ROS_INFO("Could not enter offboard mode"); - } - //last_request = ros::Time::now(); - } else { - if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ - if( arming_client.call(arm_cmd) && arm_cmd.response.success){ - ROS_INFO("Vehicle armed"); - } - last_request = ros::Time::now(); - } - } - - if(current_state.mode == "OFFBOARD" && current_state.armed){ - ROS_INFO_ONCE("Spiri is taking off"); - if(!takeoff){ - tkoff_req = ros::Time::now(); - takeoff = true; + if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ + if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ + } else { + ROS_INFO("Could not enter offboard mode"); + } + //last_request = ros::Time::now(); + } else { + if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ + if( arming_client.call(arm_cmd) && arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } } + + if(current_state.mode == "OFFBOARD" && current_state.armed){ + ROS_INFO_ONCE("Spiri is taking off"); + if(!takeoff){ + tkoff_req = ros::Time::now(); + takeoff = true; + } + } + + // Determine which messages to send + if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){ + att_targ.header.stamp = ros::Time::now(); + att_targ_pub.publish(att_targ); + att_cmds = true; + ROS_INFO("ATTITUDE CTRL"); + ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Cur Altitude: %.2f", current_altitude); + } else { + local_pos_pub.publish(buff_pose); + ROS_INFO("POSITION CTRL"); + ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Cur Altitude: %.2f", current_altitude); + } + ros::spinOnce(); + rate_pub.sleep(); } - attitude.thrust = thrust.thrust; - - // Determine which messages to send - if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){ - attitude.orientation = q_des; - attitude.header.stamp = ros::Time::now(); - att_targ.publish(attitude); - att_cmds = true; - ROS_INFO("ATTITUDE CTRL"); - ROS_INFO("Des Altitude: %.2f", alt_des); - ROS_INFO("Cur Altitude: %.2f", current_altitude); - } else { - local_pos_pub.publish(buff_pose); - ROS_INFO("POSITION CTRL"); - ROS_INFO("Des Altitude: %.2f", alt_des); - ROS_INFO("Cur Altitude: %.2f", current_altitude); - } - - // Publish euler angs - euler_pub.publish(eulAng); - - ros::spinOnce(); - rate_pub.sleep(); - } } return 0; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index ba2365b..8a8ae7d 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -8,14 +8,10 @@ import numpy as np import time import math -from pymavlink import mavutil -from scipy import signal from scipy.integrate import odeint - -from oscillation_ctrl.msg import tethered_status, RefSignal, LoadAngles -from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest -from controller_msgs.msg import FlatTarget -from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped +from oscillation_ctrl.msg import RefSignal, LoadAngles +from oscillation_ctrl.srv import WaypointTrack +from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped from sensor_msgs.msg import Imu from mavros_msgs.msg import State @@ -32,23 +28,20 @@ class Main: self.tstart = rospy.get_time() self.dt = 1.0/rate - self.tmax = self.dt + self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax) self.n = self.tmax/self.dt + 1 self.t = np.linspace(0, self.tmax, self.n) # Time array # Message generation/ collection - self.state = State() - self.mode = '' - self.vel_data = TwistStamped() # This is needed to get drone vel from gps - self.imu_data = Imu() # Needed for to get drone acc from IMU - self.ref_sig = FlatTarget() # Smooth Signal + self.vel_data = TwistStamped() # This is needed to get drone vel from gps + self.imu_data = Imu() # Needed for to get drone acc from IMU + self.ref_sig = RefSignal() # Smooth Signal self.load_angles = LoadAngles() self.has_run = 0 # Bool to keep track of first run instance self.dr_pos = Pose() self.dr_vel = self.vel_data.twist.linear self.dr_acc = self.imu_data.linear_acceleration - self.dr_acc_p = self.imu_data.linear_acceleration # Get tether length self.param_exists = False @@ -69,15 +62,14 @@ class Main: #rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) - rospy.Subscriber('/mavros/state', State, self.state_cb) # --------------------------------------------------------------------------------# # PUBLISHERS # --------------------------------------------------------------------------------# # Publish desired path to compute attitudes - self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) + self.pub_path = rospy.Publisher('/reference/path',RefSignal,queue_size = 10) # Needed for geometric controller to compute thrust - self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) + #self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) # timer(s), used to control method loop freq(s) as defined by the rate(s) self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) @@ -108,8 +100,8 @@ class Main: # For saturation: self.jmax = [3, 3, 1] - self.amax = [2, 2, 1] - self.vmax = [3, 3, 1] + self.amax = [5, 5, 1] + self.vmax = [10, 10, 1] #[3, 3, 1] self.max = [0, 3, 1.5, 3] #[0, 3, 1.5, 3] # create the array: [vmax; amax; jmax] self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T @@ -158,29 +150,8 @@ class Main: self.ak = np.zeros(len(self.SF_delay_x[0])) self.s_gain = 0.0 # Gain found from sigmoid # --------------------------------------------------------------------------------# -# CALLBACK FUNCTIONS +# CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# - def state_cb(self, data): - if self.state.armed != data.armed: - rospy.loginfo("armed state changed from {0} to {1}".format( - self.state.armed, data.armed)) - - if self.state.connected != data.connected: - rospy.loginfo("connected changed from {0} to {1}".format( - self.state.connected, data.connected)) - - if self.state.mode != data.mode: - rospy.loginfo("mode changed from {0} to {1}".format( - self.state.mode, data.mode)) - - if self.state.system_status != data.system_status: - rospy.loginfo("system_status changed from {0} to {1}".format( - mavutil.mavlink.enums['MAV_STATE'][ - self.state.system_status].name, mavutil.mavlink.enums[ - 'MAV_STATE'][data.system_status].name)) - - self.state = data - # mavros publishes a disconnected state message on init # Callback to get link names, states, and pload deflection angles def loadAngles_cb(self,msg): @@ -351,11 +322,11 @@ class Main: for i in range(9): # Populate EPS_F buffer with desired change based on feedback - self.EPS_F[i] = self.EPS_I[i] #+ EPS_D[i] #+ EPS_D[i] + self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i] # Populate msg with epsilon_final self.ref_sig.header.stamp = rospy.Time.now() - self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin + #self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin self.ref_sig.position.x = self.EPS_F[0] self.ref_sig.position.y = self.EPS_F[1] self.ref_sig.velocity.x = self.EPS_F[3] @@ -364,9 +335,9 @@ class Main: self.ref_sig.acceleration.y = self.EPS_F[7] # Do not need to evaluate z - self.ref_sig.position.z = 5.0 - self.ref_sig.velocity.z = 0 - self.ref_sig.acceleration.z = 0 + self.ref_sig.position.z = self.xd.z + self.ref_sig.velocity.z = 0.0 + self.ref_sig.acceleration.z = 0.0 #self.ref_sig.position.z = self.EPS_F[2] #self.ref_sig.velocity.z = self.EPS_F[5] #self.ref_sig.acceleration.z = self.EPS_F[8] @@ -411,7 +382,7 @@ class Main: # Publish reference signal self.pub_path.publish(self.ref_sig) - self.pub_ref.publish(self.ref_sig) + #self.pub_ref.publish(self.ref_sig) # for geometric controller # Give user feedback on published message: #self.screen_output() diff --git a/src/thrust_controller.py b/src/thrust_controller.py new file mode 100755 index 0000000..0c56345 --- /dev/null +++ b/src/thrust_controller.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez Sept 21 +### Used to control thrust of drone + +import rospy, tf +import numpy as np + +from geometry_msgs.msg import PoseStamped, TwistStamped +from mavros_msgs.msg import AttitudeTarget + +class PID: + def __init__(self): + # rate(s) + rate = 35.0 + self.dt = 1/rate + # Variables needed for testing start + self.tstart = rospy.get_time() # Keep track of the start time + while self.tstart == 0.0: # Need to make sure get_rostime works + self.tstart = rospy.get_time() + + # tuning gains + #self.Kp = 0.335 + #self.Kd = 0.1 + self.Kp = 2.7 + self.Kd = 0.5 + + # drone var + self.drone_m = 1.437 + self.max_a = 14.2 + self.max_t = self.drone_m*self.max_a + + # message variables + self.pose = PoseStamped() + self.pose_buff = PoseStamped() + self.pose_buff.pose.position.z = 2.5 + + self.attitude = AttitudeTarget() + self.attitude.type_mask = 1|2|4 # ignore body rate command + self.attitude.orientation.x = 0.0 + self.attitude.orientation.y = 0.0 + self.attitude.orientation.z = 0.0 + self.attitude.orientation.w = 1.0 + + self.des_alt = 2.5 + self.dr_vel = TwistStamped() + #self.cur_att = PoseStamped() + self.R = np.empty([3,3]) + + # clients + + + # subscribers + rospy.Subscriber('/mavros/local_position/pose',PoseStamped, self.pose_cb) + rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self.droneVel_cb) + + # publishers + self.pub_attitude = rospy.Publisher('/command/thrust', AttitudeTarget, queue_size=10) + + + # publishing rate + self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) + + def pose_cb(self,msg): + self.cur_alt = msg.pose.position.z + + self.pose = msg + + # Callback for drone velocity + def droneVel_cb(self,msg): + try: + self.dr_vel = msg + + except ValueError or TypeError: + pass + + + def quaternion_rotation_matrix(self): + """ + Covert a quaternion into a full three-dimensional rotation matrix. + + Input + :param Q: A 4 element array representing the quaternion (q0,q1,q2,q3) + + Output + :return: A 3x3 element matrix representing the full 3D rotation matrix. + This rotation matrix converts a point in the local reference + frame to a point in the global reference frame. + """ + # Extract the values from Q + q0 = self.pose.pose.orientation.w + q1 = self.pose.pose.orientation.x + q2 = self.pose.pose.orientation.y + q3 = self.pose.pose.orientation.z + + # First row of the rotation matrix + r00 = 2 * (q0 * q0 + q1 * q1) - 1 + r01 = 2 * (q1 * q2 - q0 * q3) + r02 = 2 * (q1 * q3 + q0 * q2) + + # Second row of the rotation matrix + r10 = 2 * (q1 * q2 + q0 * q3) + r11 = 2 * (q0 * q0 + q2 * q2) - 1 + r12 = 2 * (q2 * q3 - q0 * q1) + + # Third row of the rotation matrix + r20 = 2 * (q1 * q3 - q0 * q2) + r21 = 2 * (q2 * q3 + q0 * q1) + r22 = 2 * (q0 * q0 + q3 * q3) - 1 + + # 3x3 rotation matrix + rot_matrix = np.array([[r00, r01, r02], + [r10, r11, r12], + [r20, r21, r22]]) + + return rot_matrix + + def operation(self): + self.error = self.cur_alt - self.des_alt # error in z dir. + self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix + + # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) + # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch + thrust = np.dot(9.81*self.drone_m -self.Kp*self.error -self.Kd*self.dr_vel.twist.linear.z,self.R.T[2])/self.max_t + self.attitude.thrust = thrust[2] # save thurst in z-dir + + # Value needs to be between 0 - 1.0 + if self.attitude.thrust >= 1.0: + self.attitude.thrust = 1.0 + elif self.attitude.thrust <= 0.0: + self.attitude.thrust = 0.0 + + + def publisher(self,pub_time): + self.operation() # determine thrust + self.attitude.header.stamp = rospy.Time.now() + self.pub_attitude.publish(self.attitude) + + def user_feedback(self): + rospy.loginfo('Des: %.2f',self.des_alt) + rospy.loginfo('Error: %.2f',self.error) + rospy.loginfo('Throttle: %f\n',self.attitude.thrust) + +if __name__=="__main__": + + # Initiate ROS node + rospy.init_node('thrust_PID') + try: + PID() # create class object + + rospy.spin() # loop until shutdown signal + + except rospy.ROSInterruptException: + pass + + + + + + + + + + + + + + + + + + +