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

small issues fixed

This commit is contained in:
cesar 2022-03-18 13:13:22 -03:00
parent 4fbed7a57c
commit 798da36dc1
7 changed files with 40 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
launch/klausen_dampen.launch
View File

@ -31,6 +31,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
pkg="oscillation_ctrl"
type="ref_signalGen.py"
name="refSignal_node"
launch-prefix="xterm -e"
/>
<node
pkg="oscillation_ctrl"

7
launch/mocap_sim.launch
View File

@ -5,6 +5,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<launch>
<arg name="mav_name" default="spiri_mocap"/>
<arg name="command_input" default="1" />
<arg name="model" default="headless_spiri_mocap"/>
<node
pkg="oscillation_ctrl"
@ -12,12 +13,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
name="fakeMocap_node"
launch-prefix="xterm -e"
/>
<!--node
<node
pkg="oscillation_ctrl"
type="offb_node"
name="offb_node"
launch-prefix="xterm -e"
/-->
/>
<!--node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
<param name="mav_name" type="string" value="$(arg mav_name)" />
@ -32,5 +33,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<param name="Kv_z" value="6.0" />
</node-->
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/headless_spiri_mocap.launch"/>
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

38
src/LinkState.py
View File

@ -22,6 +22,8 @@ class Main:
self.dt = 1.0/rate
rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru
# 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
@ -64,14 +66,16 @@ class Main:
self.service1 = '/gazebo/get_link_state'
# need service list to check if models have spawned
self.service_list = rosservice.get_service_list()
# self.service_list = rosservice.get_service_list()
# wait for service to exist
while self.service1 not in self.service_list:
print "Waiting for models to spawn..."
self.service_list = rosservice.get_service_list()
if rospy.get_time() - self.tstart >= 10.0:
break
rospy.wait_for_service(self.service1,timeout=10)
# while self.service1 not in self.service_list:
# print "Waiting for models to spawn..."
# self.service_list = rosservice.get_service_list()
# if rospy.get_time() - self.tstart >= 10.0:
# break
# publisher(s)
self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
@ -141,7 +145,8 @@ class Main:
if not self.has_run == 1:
if self.pload == True:
# Determine yaw offset
self.yaw_offset = drone_Eul[2]
self.yaw_offset = drone_Eul[2]
# Get tether length based off initial displacement
self.tetherL = math.sqrt((drone_P.link_state.pose.position.x -
@ -164,17 +169,20 @@ class Main:
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]
#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)
#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]
#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
# Pload
@ -213,7 +221,7 @@ class Main:
# Print and save results
print "\n"
rospy.loginfo("")
print"Roll: "+str(round(drone_Eul[0],2)),"\nPitch: "+str(round(drone_Eul[1],2)),"\nYaw: "+str(round(drone_Eul[2],4))
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))

2
src/offb_node.cpp
View File

@ -114,7 +114,7 @@ int main(int argc, char **argv)
// Populate pose msg
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1.0;
pose.pose.position.z = 2.5;
pose.pose.orientation.x = q_init.x;
pose.pose.orientation.y = q_init.y;
pose.pose.orientation.z = q_init.z;

19
src/ref_signalGen.py
View File

@ -409,11 +409,13 @@ class Main:
def screen_output(self):
# Feedback to user
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('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('_______________________')
#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('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('_______________________')
rospy.loginfo('xd = %.2f',self.xd.x)
def publisher(self,pub_tim):
@ -424,12 +426,9 @@ class Main:
self.pub_path.publish(self.ref_sig)
self.pub_ref.publish(self.ref_sig)
# Give user feedback on published message:
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('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('_______________________')
self.screen_output()
if __name__=="__main__":

12
src/square.py
View File

@ -41,7 +41,6 @@ class Main:
def wait_cb(self,data):
self.bool = data
# Publish messages
def pub(self,pub_timer):
if self.bool == False:
@ -65,17 +64,6 @@ class Main:
self.j += 1
self.i = self.j // self.buffer
# self.Point.header.stamp = rospy.Time.now()
# self.Point.x = self.xarray[self.i]
# self.Point.y = self.yarray[self.i]
# rospy.loginfo("Sending [Point x] %d [Point y] %d",
# self.Point.x, self.Point.y)
# Published desired msgs
# self.pub_square.publish(self.Point)
# self.i += 1
if __name__=="__main__":
# Initiate ROS node

4
src/step.py
View File

@ -14,9 +14,9 @@ class Main:
# variable(s)
self.Point = Point()
# Init x, y, & z coordinates
self.Point.x = 1
self.Point.x = 5
self.Point.y = 0
self.Point.z = 4.0
self.Point.z = 3.5 # need to check what the xd was previously and save the height
self.bool = False