scorpio1
/
OscillationControlWithCircle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added src and launch files

This commit is contained in:
Matas Keras 2022-07-13 10:11:01 -04:00
commit 76ce88cc4e
2 changed files with 113 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
launch/oscillation_damp_circle.launch Normal file
View File

@ -0,0 +1,86 @@
<?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"/>
<arg name="turtle" 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"
/>
<!-- PUBLISHES WAYPOINTS FOR CIRCLE -->
<node
pkg="oscillation_ctrl"
type="circle_waypoints.py"
name="circle_waypoints"
/>
<group if="$(eval turtle != 'none')">
<!-- TAKES DRONE POSITION, AND PASSES IT TO TURTLE-->
<node
pkg="oscillation_ctrl"
type="turtle_test.py"
name="turtle_test"
/>
<!-- TURTLE NODE -->
<node
pkg="turtlesim"
type="turtlesim_node"
name="turtlesim_node"
/>
</group>
<!-- 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 oscillation_ctrl)/launch/$(arg model).launch"/>
</launch>

27
src/circle_waypoints.py Executable file
View File

@ -0,0 +1,27 @@
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Point
import numpy as np
def circle_points():
pub = rospy.Publisher('reference/waypoints',Point, queue_size=10)
rospy.init_node('circle_points', anonymous=True)
rospy.loginfo('init_node')
rate=rospy.Rate(10)
rospy.sleep(48.)
now = rospy.get_rostime().secs
while not rospy.is_shutdown():
x_pos = np.cos((rospy.get_rostime().secs-now)*np.pi/30)-1
y_pos = np.sin((rospy.get_rostime().secs-now)*np.pi/30)
z_pos = 5
to_publish = Point(x=x_pos,y=y_pos,z=z_pos)
pub.publish(to_publish)
rate.sleep
if __name__ == '__main__':
try:
circle_points()
except rospy.ROSInterruptException:
pass