diff --git a/.gitignore b/.gitignore
index acf4bf1..eae16d7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,3 @@
-
+launch/debug.launch
src/MoCap_*.py
+*.rviz
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c7b76b..17af69a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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(
diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml
index b49f087..96d5581 100644
--- a/config/Ctrl_param.yaml
+++ b/config/Ctrl_param.yaml
@@ -1,3 +1,5 @@
# Ros param when using Klausen Ctrl
-wait: 52
\ No newline at end of file
+wait: 52
+waypoints: {x: 0.0, y: 0.0, z: 3.0}
+
diff --git a/config/noCtrl_param.yaml b/config/noCtrl_param.yaml
index fa41ec4..92b4b98 100644
--- a/config/noCtrl_param.yaml
+++ b/config/noCtrl_param.yaml
@@ -1,3 +1,4 @@
# Ros param when not using Klausen Ctrl
wait: 55
+waypoints: {x: 0.0, y: 0.0, z: 3.0}
diff --git a/frames.gv b/frames.gv
new file mode 100644
index 0000000..3ab6af0
--- /dev/null
+++ b/frames.gv
@@ -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";
+}
\ No newline at end of file
diff --git a/frames.pdf b/frames.pdf
new file mode 100644
index 0000000..455ad02
Binary files /dev/null and b/frames.pdf differ
diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch
index 5ee05bb..ac9b4e6 100644
--- a/launch/klausen_dampen.launch
+++ b/launch/klausen_dampen.launch
@@ -10,6 +10,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
+
@@ -59,5 +60,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
-
-
\ No newline at end of file
+
+
diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch
new file mode 100644
index 0000000..0cc5597
--- /dev/null
+++ b/launch/mocap_sim.launch
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/takeoff_noCtrl.launch b/launch/takeoff_noCtrl.launch
index 87404cf..56ecfe7 100644
--- a/launch/takeoff_noCtrl.launch
+++ b/launch/takeoff_noCtrl.launch
@@ -1,7 +1,7 @@
-
+
@@ -24,4 +24,7 @@
name="test_node"
launch-prefix="xterm -e"
/>
+
+
+
diff --git a/src/LinkState.py b/src/LinkState.py
index 3012bd9..7e6c6ef 100755
--- a/src/LinkState.py
+++ b/src/LinkState.py
@@ -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'
diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py
index 8c207fb..a38ac54 100755
--- a/src/MoCap_Localization_noTether.py
+++ b/src/MoCap_Localization_noTether.py
@@ -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__":
diff --git a/src/klausen_control.py b/src/klausen_control.py
index 28199d8..0f1d61c 100755
--- a/src/klausen_control.py
+++ b/src/klausen_control.py
@@ -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]
diff --git a/src/offb_node.cpp b/src/offb_node.cpp
index 9e2f61c..9398b68 100644
--- a/src/offb_node.cpp
+++ b/src/offb_node.cpp
@@ -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;
diff --git a/src/path_follow.cpp b/src/path_follow.cpp
index d3d4a58..fe2afea 100644
--- a/src/path_follow.cpp
+++ b/src/path_follow.cpp
@@ -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 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;
diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py
index 2fa367d..889a931 100755
--- a/src/ref_signalGen.py
+++ b/src/ref_signalGen.py
@@ -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
# --------------------------------------------------------------------------------#
diff --git a/src/step.py b/src/step.py
index d90c4fb..e229107 100755
--- a/src/step.py
+++ b/src/step.py
@@ -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
diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py
new file mode 100755
index 0000000..0a7dcd4
--- /dev/null
+++ b/src/wpoint_tracker.py
@@ -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
+
diff --git a/srv/WaypointTrack.srv b/srv/WaypointTrack.srv
new file mode 100644
index 0000000..7352ec2
--- /dev/null
+++ b/srv/WaypointTrack.srv
@@ -0,0 +1,4 @@
+bool query # bool to request waypoints
+---
+geometry_msgs/Point xd # waypoints
+