forked from cesar.alejandro/oscillation_ctrl
Mavros Controllers is no longer a dependency of this package
This commit is contained in:
parent
71f7574383
commit
ef4ea858ea
|
|
@ -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
|
||||
)
|
||||
|
||||
################################################
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,62 @@
|
|||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Launch file to use klausen oscillaton damping ctrl in Gazebo
|
||||
/-->
|
||||
<launch>
|
||||
<arg name="model" default="headless_spiri_with_tether"/>
|
||||
<arg name='test' default="none"/>
|
||||
|
||||
<group ns="sim">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
|
||||
</group>
|
||||
|
||||
<group ns="status">
|
||||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="LinkState.py"
|
||||
name="linkStates_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED POSITION -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
/>
|
||||
<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="ref_signalGen.py"
|
||||
name="refSignal_node"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
output="screen"
|
||||
/>
|
||||
<!-- PUBLISHES DESIRED COMMANDS -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="pathFollow_node"
|
||||
name="pathFollow_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
||||
<!-- RUNS DIFFERENT TESTS IF DESIRED -->
|
||||
<group if="$(eval test != 'none')">
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="perform_test.py"
|
||||
name="test_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</group>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||
</launch>
|
||||
|
|
@ -1,10 +1,13 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="test_type" default="step.py" />
|
||||
<arg name="model" default="spiri"/>
|
||||
<arg name="model" default="headless_spiri_with_tether"/>
|
||||
<arg name='test' default="none"/>
|
||||
<group ns="sim">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
|
||||
</group>
|
||||
<group ns="status">
|
||||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
@ -18,13 +21,22 @@
|
|||
name="offb_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="$(arg test_type)"
|
||||
name="test_node"
|
||||
launch-prefix="xterm -e"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
/>
|
||||
|
||||
<group if="$(eval test != 'none')">
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="perform_test.py"
|
||||
name="test_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</group>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
pass
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <mavros_msgs/Thrust.h>
|
||||
#include <mavros_msgs/VFR_HUD.h>
|
||||
#include <oscillation_ctrl/WaypointTrack.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -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<geometry_msgs::PoseStamped>
|
||||
("/reference/waypoints",10,waypoints_cb);
|
||||
|
||||
*/
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
("status/waypoint_tracker");
|
||||
|
||||
/********************** PUBLISHERS **********************/
|
||||
// Initiate publisher to publish commanded local position
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
("mavros/setpoint_position/local", 10);
|
||||
|
||||
// Publish desired attitude
|
||||
ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
|
||||
("mavros/setpoint_attitude/thrust", 10);
|
||||
("mavros/setpoint_attitude/thrust", 10);
|
||||
|
||||
// Publish attitude commands
|
||||
ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
|
|
@ -96,9 +95,9 @@ int main(int argc, char **argv)
|
|||
|
||||
// Service Clients
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("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);
|
||||
|
||||
|
|
|
|||
|
|
@ -10,54 +10,37 @@
|
|||
#include <oscillation_ctrl/WaypointTrack.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/CommandTOL.h>
|
||||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <mavros_msgs/Thrust.h>
|
||||
#include <mavros_msgs/VFR_HUD.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <iostream>
|
||||
#include <cmath> // 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_msgs::State>
|
||||
("mavros/state", 10, state_cb);
|
||||
|
||||
// Get quaternions from klausen control
|
||||
ros::Subscriber q_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("command/quaternions",10,q_cb);
|
||||
// Get attitude target from klausen control
|
||||
ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
|
||||
("command/att_target",10,att_targ_cb);
|
||||
|
||||
// Pose subscriber
|
||||
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("mavros/local_position/pose",10,mavPose_cb);
|
||||
|
||||
// Throttle: Jaeyoung-Lim
|
||||
ros::Subscriber thro_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
|
||||
("command/bodyrate_command",10,thrust_cb);
|
||||
|
||||
/********************** PUBLISHERS **********************/
|
||||
// Initiate publisher to publish commanded local position
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
|
||||
// Publish attitude and thrust commands
|
||||
ros::Publisher att_targ = nh.advertise<mavros_msgs::AttitudeTarget>
|
||||
ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
|
||||
("mavros/setpoint_raw/attitude",10);
|
||||
|
||||
// Publish attitude cmds in euler angles
|
||||
ros::Publisher euler_pub = nh.advertise<oscillation_ctrl::EulerAngles>
|
||||
("command/euler_angles",10);
|
||||
|
||||
// Service Clients
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("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;
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue