scorpio1
/
oscillation_ctrl
forked from cesar.alejandro/oscillation_ctrl
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Controller now uses MAVROS for localization

This commit is contained in:
cesar.alejnadro 2022-04-13 14:41:13 -03:00
parent 08bcb9fbc9
commit ee7ec83869
5 changed files with 149 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
launch/klausen_dampen.launch
View File

@ -25,13 +25,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="LinkState.py" type="LinkState.py"
name="linkStates_node" name="linkStates_node"
launch-prefix="xterm -fa 'Monospace' -fs 18 -e" launch-prefix="xterm -e"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="wpoint_tracker.py" type="wpoint_tracker.py"
name="waypoints_server" name="waypoints_server"
launch-prefix="xterm -e"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
@ -52,11 +51,13 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
launch-prefix="xterm -e" launch-prefix="xterm -e"
/> />
<group if="$(eval test != 'none')">
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
type="perform_test.py" type="perform_test.py"
name="test_node" name="test_node"
/> />
</group>
<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen"> <node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
<param name="mav_name" type="string" value="$(arg mav_name)" /> <param name="mav_name" type="string" value="$(arg mav_name)" />
<!--remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/--> <!--remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/-->

60
src/LinkState.py
View File

@ -19,7 +19,6 @@ class Main:
# rate(s) # rate(s)
rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz
self.dt = 1.0/rate self.dt = 1.0/rate
rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru
@ -41,9 +40,6 @@ class Main:
# Will be set to true when test should start # Will be set to true when test should start
self.bool = False self.bool = False
# Vehicle is spawned with yaw offset for convenience, need to deal with that
self.yaw_offset = 0.0
# variables for message gen # variables for message gen
self.status = tethered_status() self.status = tethered_status()
self.drone_id = 'spiri_with_tether::spiri::base' self.drone_id = 'spiri_with_tether::spiri::base'
@ -74,6 +70,7 @@ class Main:
self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state) self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow) self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
self.gui_timer = rospy.Timer(rospy.Duration(1/10.0), self.user_feedback)
def cutoff(self,value,ceiling): def cutoff(self,value,ceiling):
""" """
@ -123,7 +120,7 @@ class Main:
drone_P = get_P(self.drone_id,reference) drone_P = get_P(self.drone_id,reference)
# Get orientation of drone in euler angles to determine yaw offset # Get orientation of drone in euler angles to determine yaw offset
drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) self.drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
# Check if payload is part of simulation # Check if payload is part of simulation
if not drone_P.success: if not drone_P.success:
@ -136,7 +133,7 @@ class Main:
if not self.has_run == 1: if not self.has_run == 1:
if self.pload == True: if self.pload == True:
# Determine yaw offset # Determine yaw offset
self.yaw_offset = drone_Eul[2] self.yaw_offset = self.drone_Eul[2]
# Get tether length based off initial displacement # Get tether length based off initial displacement
@ -155,25 +152,9 @@ class Main:
# Need to detemine their location to get angle of deflection # Need to detemine their location to get angle of deflection
# Drone # Drone
drone_Px = drone_P.link_state.pose.position.x self.drone_Px = drone_P.link_state.pose.position.x
drone_Py = drone_P.link_state.pose.position.y self.drone_Py = drone_P.link_state.pose.position.y
drone_Pz = drone_P.link_state.pose.position.z self.drone_Pz = drone_P.link_state.pose.position.z
# Get drone orientation
#drone_q = [drone_P.link_state.pose.orientation.x,drone_P.link_state.pose.orientation.y,
# drone_P.link_state.pose.orientation.z,drone_P.link_state.pose.orientation.w]
# offset orientation by yaw offset
#q_offset = quaternion_from_euler(0,0,-self.yaw_offset)
#drone_q = quaternion_multiply(drone_q,q_offset)
#drone_P.link_state.pose.orientation.x = drone_q[0]
#drone_P.link_state.pose.orientation.y = drone_q[1]
#drone_P.link_state.pose.orientation.z = drone_q[2]
#drone_P.link_state.pose.orientation.w = drone_q[3]
# Get euler angles again for feedback to user
#drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
if self.pload == True: # If there is payload, determine the variables if self.pload == True: # If there is payload, determine the variables
# Pload # Pload
@ -181,7 +162,7 @@ class Main:
pload_Py = pload_P.link_state.pose.position.y pload_Py = pload_P.link_state.pose.position.y
# Determine theta (pitch) # Determine theta (pitch)
x_sep = pload_Px - drone_Px x_sep = pload_Px - self.drone_Px
if math.fabs(x_sep) >= self.tetherL or x_sep == 0: if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
self.loadAngles.theta = 0 self.loadAngles.theta = 0
@ -194,12 +175,12 @@ class Main:
self.thetabuf = self.loadAngles.theta self.thetabuf = self.loadAngles.theta
# Determine phi (roll) # Determine phi (roll)
y_sep = pload_Py - drone_Py y_sep = pload_Py - self.drone_Py
if math.fabs(y_sep) >= self.tetherL or y_sep == 0: if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
self.loadAngles.phi = 0 self.loadAngles.phi = 0
else: else:
self.loadAngles.phi = math.asin(y_sep/self.tetherL) self.loadAngles.phi = -math.asin(y_sep/self.tetherL)
# Determine phidot # Determine phidot
self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt self.loadAngles.phidot = (self.loadAngles.phi - self.phibuf)/self.dt
@ -209,16 +190,6 @@ class Main:
else: # Otherwise, vars = 0 else: # Otherwise, vars = 0
x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0 x_sep = self.loadAngles.phi = self.loadAngles.phidot = self.loadAngles.theta = self.thetadot = 0
# Print and save results
print "\n"
rospy.loginfo("")
print"Roll: "+str(round(drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(drone_Eul[2]*180/3.14,2))
print "drone pos.x: " + str(round(drone_Px,2))
print "drone pos.y: " + str(round(drone_Py,2))
print "drone pos.z: " + str(round(drone_Pz,2))
print "phi: " + str(round(self.loadAngles.phi*180/3.14,3))
print "theta: " + str(round(self.loadAngles.theta*180/3.14,3))
# Populate message # Populate message
self.status.header.stamp = rospy.Time.now() self.status.header.stamp = rospy.Time.now()
self.status.drone_pos = drone_P.link_state.pose self.status.drone_pos = drone_P.link_state.pose
@ -232,6 +203,17 @@ class Main:
except rospy.ServiceException as e: except rospy.ServiceException as e:
rospy.loginfo("Get Link State call failed: {0}".format(e)) rospy.loginfo("Get Link State call failed: {0}".format(e))
def user_feedback(self,gui_timer):
# 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 "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))
print "phi: " + str(round(self.loadAngles.phi*180/3.14,3))
print "theta: " + str(round(self.loadAngles.theta*180/3.14,3))
def path_follow(self,path_timer): def path_follow(self,path_timer):
now = rospy.get_time() now = rospy.get_time()
if now - self.tstart < self.wait: if now - self.tstart < self.wait:
@ -240,7 +222,6 @@ class Main:
self.bool = True self.bool = True
self.pub_wd.publish(self.bool) self.pub_wd.publish(self.bool)
if __name__=="__main__": if __name__=="__main__":
# Initiate ROS node # Initiate ROS node
@ -251,4 +232,3 @@ if __name__=="__main__":
except rospy.ROSInterruptException: except rospy.ROSInterruptException:
pass pass

20
src/klausen_control.py
View File

@ -47,7 +47,7 @@ class Main:
self.path_vel = np.zeros([3,1]) self.path_vel = np.zeros([3,1])
self.path_acc = np.zeros([3,1]) self.path_acc = np.zeros([3,1])
self.dr_pos = Pose() self.dr_pos = Pose()
self.quaternion = PoseStamped() self.quaternion = PoseStamped() # used to send quaternion attitude commands
self.load_angles = LoadAngles() self.load_angles = LoadAngles()
self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q
@ -102,6 +102,8 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/reference/path', FlatTarget, self.refsig_cb) rospy.Subscriber('/reference/path', FlatTarget, 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/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
@ -133,9 +135,10 @@ class Main:
# Callback drone pose # Callback drone pose
def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub def dronePos_cb(self,msg):
try: try:
self.dr_pos = msg.pose self.dr_pos = msg.pose
#self.dr_pos = msg.drone_pos
except ValueError: except ValueError:
pass pass
@ -211,23 +214,15 @@ class Main:
# Control matrices - this may be better in _init_ # Control matrices - this may be better in _init_
M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta], M = [[self.tot_m, 0, 0, 0, L*self.pl_m*c_theta],
[0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta], [0, self.tot_m, 0, -L*self.pl_m*c_phi*c_theta, L*self.pl_m*s_phi*s_theta],
[0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta], [0, 0, self.tot_m, -L*self.pl_m*c_theta*s_phi, -L*self.pl_m*c_phi*s_theta],
[0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0], [0, -L*self.pl_m*c_phi*c_theta,-L*self.pl_m*c_theta*s_phi, (L**2)*self.pl_m*c_theta**2 + 0.01*s_theta**2, 0],
[L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]] [L*self.pl_m*c_theta, L*self.pl_m*s_phi*s_theta, -L*self.pl_m*c_phi*s_theta, 0, L**2*self.pl_m]]
C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta], C = [[0,0,0,0,-L*self.load_angles.thetadot*self.pl_m*s_theta],
[0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)], [0,0,0,L*self.pl_m*(self.load_angles.phidot*c_theta*s_phi + self.load_angles.thetadot*c_phi*s_theta), L*self.pl_m*(self.load_angles.phidot*c_phi*s_theta + self.load_angles.thetadot*c_theta*s_phi)],
[0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)], [0,0,0,-L*self.pl_m*(self.load_angles.phidot*c_phi*c_theta - self.load_angles.thetadot*s_phi*s_theta),-L*self.pl_m*(self.load_angles.thetadot*c_phi*c_theta - self.load_angles.phidot*s_phi*s_theta)],
[0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)], [0,0,0,-0.5*(L**2)*self.pl_m*self.load_angles.thetadot*math.sin(2*self.load_angles.theta) + 0.5*0.01*self.load_angles.thetadot*math.sin(2*self.load_angles.theta), -0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta)],
[0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]] [0,0,0,0.5*(L**2)*self.pl_m*self.load_angles.phidot*math.sin(2*self.load_angles.theta),0]]
G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]] G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]]
@ -290,6 +285,7 @@ class Main:
Ki = self.tune*self.K1 Ki = self.tune*self.K1
# Desired body-oriented forces # Desired body-oriented forces
# shouldnt it be tot_m*path_acc?
Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p)) Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p))
# Update self.z1_p for "integration" # Update self.z1_p for "integration"
@ -302,8 +298,8 @@ class Main:
Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0],dr_orientation_inv)) # Real part of Fd needs = 0 Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0],dr_orientation_inv)) # Real part of Fd needs = 0
# Convert forces to attiude *EulerAng[2] = yaw = 0 # Convert forces to attiude *EulerAng[2] = yaw = 0
self.EulerAng[1] = math.atan(-Fd_tf[0]/(self.drone_m*9.81)) # Pitch -- maybe self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch
self.EulerAng[0] = math.atan(Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll -- maybe self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll
# Convert Euler angles to quaternions # Convert Euler angles to quaternions
q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2])

2
src/path_follow.cpp
View File

@ -229,7 +229,7 @@ int main(int argc, char **argv)
attitude.thrust = thrust.thrust; attitude.thrust = thrust.thrust;
// Determine which messages to send // Determine which messages to send
if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){ if(ros::Time::now() - tkoff_req > ros::Duration(30.0) && takeoff){
attitude.orientation = q_des; attitude.orientation = q_des;
attitude.header.stamp = ros::Time::now(); attitude.header.stamp = ros::Time::now();
att_targ.publish(attitude); att_targ.publish(attitude);

34
src/ref_signalGen.py
View File

@ -32,8 +32,6 @@ class Main:
self.tstart = rospy.get_time() self.tstart = rospy.get_time()
self.dt = 1.0/rate self.dt = 1.0/rate
# self.dt = 0.5
# self.tmax = 100
self.tmax = self.dt self.tmax = self.dt
self.n = self.tmax/self.dt + 1 self.n = self.tmax/self.dt + 1
self.t = np.linspace(0, self.tmax, self.n) # Time array self.t = np.linspace(0, self.tmax, self.n) # Time array
@ -68,6 +66,7 @@ class Main:
# Topic, msg type, and class callback method # Topic, msg type, and class callback method
rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb)
#rospy.Subscriber('/status/twoBody_status', tethered_status, self.dronePos_cb)
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
rospy.Subscriber('/mavros/state', State, self.state_cb) rospy.Subscriber('/mavros/state', State, self.state_cb)
@ -92,7 +91,7 @@ class Main:
self.EPS_I = np.zeros(9) # Epsilon shapefilter self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation # Constants for smooth path generation
self.w_tune = 3.13 # 3.13 works well? ######################################################################### self.w_tune = 3.5 # 3.13 works well? #########################################################################
self.epsilon = 0.7 # Damping ratio self.epsilon = 0.7 # Damping ratio
# need exception if we do not have tether: # need exception if we do not have tether:
@ -115,13 +114,6 @@ class Main:
# create the array: [vmax; amax; jmax] # create the array: [vmax; amax; jmax]
self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T
# Desired position array
#if rospy.has_param('sim/waypoints'):
# self.xd = rospy.get_param('sim/waypoints') # waypoints
#elif rospy.get_time() - self.tstart >= 3.0:
# self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints
#self.xd = Point()
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
self.empty_point = Point() # Needed to query waypoint_server self.empty_point = Point() # Needed to query waypoint_server
@ -198,9 +190,10 @@ class Main:
pass pass
# Callback drone pose # Callback drone pose
def dronePos_cb(self,msg): ### NEED to add mavros/local_pos.. sub def dronePos_cb(self,msg):
try: try:
self.dr_pos = msg.pose self.dr_pos = msg.pose
#self.dr_pos = msg.drone_pos
except ValueError: except ValueError:
pass pass
@ -229,11 +222,11 @@ class Main:
except ValueError: except ValueError:
pass pass
################################################################# ######################################################################
#TODO Will need to add a function that gets a message from # #TODO Will need to add a function that gets a message from #
# a node which lets refsignal_gen.py know there has been a # # a node which lets refsignal_gen.py know there has been a #
# change in xd and therefore runs waypoints_srv_cb again # # change in xd and therefore runs waypoints_srv_cb again #
################################################################# ######################################################################
# --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------#
# ALGORITHM # ALGORITHM
@ -365,7 +358,8 @@ class Main:
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.x = self.EPS_F[0]
self.ref_sig.position.y = self.EPS_F[1] self.ref_sig.position.y = self.EPS_F[1]
self.ref_sig.position.z = self.EPS_F[2] #self.ref_sig.position.z = self.EPS_F[2]
self.ref_sig.position.z = 5.0
self.ref_sig.velocity.x = self.EPS_F[3] self.ref_sig.velocity.x = self.EPS_F[3]
self.ref_sig.velocity.y = self.EPS_F[4] self.ref_sig.velocity.y = self.EPS_F[4]
self.ref_sig.velocity.z = self.EPS_F[5] self.ref_sig.velocity.z = self.EPS_F[5]
@ -398,13 +392,13 @@ class Main:
def screen_output(self): def screen_output(self):
# Feedback to user # Feedback to user
#rospy.loginfo(' Var | x | y | z ') rospy.loginfo(' Var | x | y | z ')
#rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
#rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
#rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
#rospy.loginfo('_______________________') rospy.loginfo('_______________________')
rospy.loginfo('xd = %.2f',self.xd.x) #rospy.loginfo('xd = %.2f',self.xd.x)
def publisher(self,pub_tim): def publisher(self,pub_tim):