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

added service node to keep track of desired waypoints

This commit is contained in:
cesar 2022-03-14 16:09:30 -03:00
parent 93744cf280
commit 0c0764387b
18 changed files with 200 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -1,2 +1,3 @@
launch/debug.launch
src/MoCap_*.py
*.rviz

10
CMakeLists.txt
View File

@ -10,6 +10,7 @@ project(oscillation_ctrl)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
mavros
message_generation
message_runtime
roscpp
)
@ -56,11 +57,10 @@ add_message_files(
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
add_service_files(
FILES
WaypointTrack.srv
)
## Generate actions in the 'action' folder
# add_action_files(

4
config/Ctrl_param.yaml
View File

@ -1,3 +1,5 @@
# Ros param when using Klausen Ctrl
wait: 52
wait: 52
waypoints: {x: 0.0, y: 0.0, z: 3.0}

1
config/noCtrl_param.yaml
View File

@ -1,3 +1,4 @@
# Ros param when not using Klausen Ctrl
wait: 55
waypoints: {x: 0.0, y: 0.0, z: 3.0}

9
frames.gv Normal file
View File

@ -0,0 +1,9 @@
digraph G {
"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
edge [style=invis];
subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
"Recorded at time: 124.428"[ shape=plaintext ] ;
}->"local_origin";
}

BIN
frames.pdf Normal file
View File

Binary file not shown.

5
launch/klausen_dampen.launch
View File

@ -10,6 +10,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<arg name="respawn_mavros" default="false" />
<arg name="gazebo_gui" default="false" />
<arg name="test_type" default="step.py" />
<arg name="model" default="spiri_with_tether"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
@ -59,5 +60,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/spiri_with_tether.launch"/>
</launch>
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

36
launch/mocap_sim.launch Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<!--
Launch file to use klausen oscillaton damping ctrl in Gazebo
/-->
<launch>
<arg name="mav_name" default="spiri_mocap"/>
<arg name="command_input" default="1" />
<node
pkg="oscillation_ctrl"
type="MoCap_Localization_fake.py"
name="fakeMocap_node"
launch-prefix="xterm -e"
/>
<!--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)" />
<!-remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/->
<param name="ctrl_mode" value="$(arg command_input)" />
<param name="max_acc" value="8.0" />
<param name="Kp_x" value="8.0" />
<param name="Kp_y" value="8.0" />
<param name="Kp_z" value="10.0" />
<param name="Kv_x" value="3.0" />
<param name="Kv_y" value="3.0" />
<param name="Kv_z" value="6.0" />
</node-->
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/headless_spiri_mocap.launch"/>
</launch>

5
launch/takeoff_noCtrl.launch
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<launch>
<arg name="test_type" default="step.py" />
<arg name="model" default="spiri"/>
<group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
</group>
@ -24,4 +24,7 @@
name="test_node"
launch-prefix="xterm -e"
/>
<!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/>
</launch>

5
src/LinkState.py
View File

@ -11,6 +11,7 @@ from tf.transformations import *
from offboard_ex.msg import tethered_status
from geometry_msgs.msg import Pose
from gazebo_msgs.srv import GetLinkState
from oscillation_ctrl.srv import WaypointTrack
from std_msgs.msg import Bool
class Main:
@ -55,11 +56,7 @@ class Main:
# variables for message gen
self.status = tethered_status()
self.status.drone_id = 'spiri_with_tether::spiri::base'
self.status.drone_pos = Pose()
self.status.pload_id = 'spiri_with_tether::mass::payload'
self.status.pload_pos = Pose()
self.status.phi = 0.0
self.status.theta = 0.0
# service(s)
self.service1 = '/gazebo/get_link_state'

36
src/MoCap_Localization_noTether.py
View File

@ -41,8 +41,19 @@ class Main:
### -*-*-*- END -*-*-*- ###
# initialize variables
self.buff_pose = PoseStamped()
#self.buff_pose1 = PoseStamped()
self.drone_pose = PoseStamped()
self.buff_pose = PoseStamped()
# Debugging
# self.buff_pose.pose.position.y = 0
# self.buff_pose.pose.position.x = 0
# self.buff_pose.pose.position.z = 0
# self.buff_pose.pose.orientation.y = 0
# self.buff_pose.pose.orientation.x = 0
# self.buff_pose.pose.orientation.z = 0
# self.buff_pose.pose.orientation.w = 1
# self.buff_pose.header.frame_id = 'map'
# Max dot values to prevent 'blowup'
self.phidot_max = 3.0
@ -110,15 +121,18 @@ class Main:
'''
Transforms mocap reading to proper coordinate frame
'''
self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
self.drone_pose = self.buff_pose
self.drone_pose.header.frame_id = "map"
# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
# Keep the w same and change x, y, and z as above.
self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
# def path_follow(self,path_timer):
# now = rospy.get_time()
@ -138,6 +152,12 @@ class Main:
def publisher(self,pub_timer):
self.FRD_Transform()
self.pose_pub.publish(self.drone_pose)
print "\n"
rospy.loginfo("")
print "drone pos.x: " + str(round(self.drone_pose.pose.position.x,2))
print "drone pos.y: " + str(round(self.drone_pose.pose.position.y,2))
print "drone pos.z: " + str(round(self.drone_pose.pose.position.z,2))
if __name__=="__main__":

8
src/klausen_control.py
View File

@ -75,7 +75,7 @@ class Main:
self.K1 = np.identity(3)
self.K2 = np.identity(5)
# Values which require updates *_p = prior
# Values which require updates. *_p = prior
self.z1_p = np.zeros([3,1])
self.a45_0 = np.zeros(2)
self.alpha = np.zeros([5,1])
@ -308,9 +308,9 @@ class Main:
# Attitude control
self.quaternion.header.stamp = rospy.Time.now()
self.quaternion.pose.position.x = 0
self.quaternion.pose.position.y = 0
self.quaternion.pose.position.z = 5.0
# self.quaternion.pose.position.x = 0
# self.quaternion.pose.position.y = 0
# self.quaternion.pose.position.z = 5.0
self.quaternion.pose.orientation.x = q[0]
self.quaternion.pose.orientation.y = q[1]
self.quaternion.pose.orientation.z = q[2]

2
src/offb_node.cpp
View File

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

10
src/path_follow.cpp
View File

@ -144,10 +144,14 @@ int main(int argc, char **argv)
attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO
attitude.type_mask = 1|2|4; // Ignore body rates
// Retrieve parameters from Ctrl_param.yaml file
/*std::vector<double> waypoints;
nh.getParam("sim/waypoints",waypoints);
// Need to send pose commands until throttle has been established
buff_pose.pose.position.x = 0.0;
buff_pose.pose.position.y = 0.0;
buff_pose.pose.position.z = 4.5;
buff_pose.pose.position.x = waypoints(1);
buff_pose.pose.position.y = waypoints(2);
buff_pose.pose.position.z = waypoints(3);*/
// Desired mode is offboard
mavros_msgs::SetMode offb_set_mode;

27
src/ref_signalGen.py
View File

@ -15,7 +15,7 @@ from oscillation_ctrl.msg import tethered_status, RefSignal
from controller_msgs.msg import FlatTarget
from geometry_msgs.msg import Pose, Vector3, PoseStamped
from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import Imu
@ -58,6 +58,7 @@ class Main:
self.has_run = 0 # Bool to keep track of first run instance
self.drone_id = ''
self.pload_id = ''
self.pl_pos = Pose()
self.dr_pos = Pose()
self.dr_vel = self.vel_data.twist.linear
@ -76,7 +77,7 @@ class Main:
self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation
self.w_tune = 3.0 # 3.13 works well? #########################################################################
self.w_tune = 3.13 # 3.13 works well? #########################################################################
self.epsilon = 0.7 # Damping ratio
self.wn = math.sqrt(9.81/self.tetherL)
self.wd = self.wn*math.sqrt(1 - self.epsilon**2)
@ -94,7 +95,15 @@ class Main:
self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T
# Desired position array
self.xd = np.array([[0],[0],[5.0]])
#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.des_waypoints = True # True = changing waypoints
# Initial conditions: [pos, vel, acc, jerk]
@ -148,7 +157,7 @@ class Main:
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)
rospy.Subscriber('/reference/waypoints',PoseStamped, self.waypoints_cb)
#rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position
# --------------------------------------------------------------------------------#
# PUBLISHERS
@ -220,15 +229,15 @@ class Main:
except ValueError:
pass
def waypoints_cb(self,msg):
def waypoints_srv_cb(self):
rospy.wait_for_service('status/waypoint_tracker')
try:
if self.des_waypoints == True:
self.xd[0] = msg.pose.position.x
self.xd[1] = msg.pose.position.y
#self.xd[2] = msg.pose.position.z
self.xd = self.get_xd(True)
# if self.des_waypoints == True:
except ValueError:
pass
# --------------------------------------------------------------------------------#
# ALGORITHM
# --------------------------------------------------------------------------------#

6
src/step.py
View File

@ -4,7 +4,7 @@
### Generate step input in x or y direction
import rospy, tf
import time
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Point
from std_msgs.msg import Bool
class Main:
@ -12,7 +12,7 @@ class Main:
def __init__(self):
# variable(s)
self.Point = PoseStamped()
self.Point = Point()
# Init x, y, & z coordinates
self.Point.pose.position.x = 1
self.Point.pose.position.y = 0
@ -24,7 +24,7 @@ class Main:
rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)
# publisher(s), with specified topic, message type, and queue_size
self.pub_step = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5)
self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5)
# rate(s)
pub_rate = 1 # rate for the publisher method, specified in Hz

71
src/wpoint_tracker.py Executable file
View File

@ -0,0 +1,71 @@
#!/usr/bin/env python2.7
### Cesar Rodriguez March 2022
### Reads waypoints from .yaml file and keeps track of them
import rospy, tf
import rosservice
import time
from geometry_msgs.msg import Point,Pose
from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse
class Main:
def __init__(self):
# 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()
# initialize variables
self.xd = Point() # used to in response
self.wpoints = [] # used to save waypoint from yaml file
self.resp = WaypointTrackResponse()
self.param_exists = False # check in case param does not exist
while self.param_exists == False:
if rospy.has_param('sim/waypoints'):
self.wpoints = rospy.get_param('sim/waypoints') # get waypoints
self.xd.x = self.wpoints['x']
self.xd.y = self.wpoints['y']
self.xd.z = self.wpoints['z']
self.param_exists = True
elif rospy.get_time() - self.tstart >= 3.0:
self.xd.x = 0.0
self.xd.y = 0.0
self.xd.z = 5.0
# service(s)
# subscriber(s)
rospy.Subscriber('/reference/waypoints',Point, self.waypoints_cb)
# publisher(s)
def waypoint_relay(self,req):
self.resp.xd = self.xd
return self.resp
# Change desired position if there is a change
def waypoints_cb(self,msg):
try:
self.xd = msg
except ValueError or TypeError:
pass
if __name__=="__main__":
# Initiate ROS node
rospy.init_node('waypoints_server',anonymous=False)
try:
obj = Main() # create class object
s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)
rospy.spin() # loop until shutdown signal
except rospy.ROSInterruptException:
pass

4
srv/WaypointTrack.srv Normal file
View File

@ -0,0 +1,4 @@
bool query # bool to request waypoints
---
geometry_msgs/Point xd # waypoints