Added service to set waypoints

config/px4_config.yaml

@@ -0,0 +1,270 @@
+# Common configuration for PX4 autopilot
+#
+# node:
+startup_px4_usb_quirk: true
+
+# --- system plugins ---
+
+# sys_status & sys_time connection options
+conn:
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  timeout: 10.0          # hertbeat timeout in seconds
+  timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
+  system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
+
+# sys_status
+sys:
+  min_voltage: 10.0   # diagnostics min voltage
+  disable_diag: false # disable all sys_status diagnostics, except heartbeat
+
+# sys_time
+time:
+  time_ref_source: "fcu"  # time_reference source
+  timesync_mode: MAVLINK
+  timesync_avg_alpha: 0.6 # timesync averaging factor
+
+# --- mavros plugins (alphabetical order) ---
+
+# 3dr_radio
+tdr_radio:
+  low_rssi: 40  # raw rssi lower level for diagnostics
+
+# actuator_control
+# None
+
+# command
+cmd:
+  use_comp_id_system_control: false # quirk for some old FCUs
+
+# dummy
+# None
+
+# ftp
+# None
+
+# global_position
+global_position:
+  frame_id: "map"             # origin frame
+  child_frame_id: "base_link" # body-fixed frame
+  rot_covariance: 99999.0   # covariance for attitude?
+  gps_uere: 1.0             # User Equivalent Range Error (UERE) of GPS sensor (m)
+  use_relative_alt: true    # use relative altitude for local coordinates
+  tf:
+    send: false               # send TF?
+    frame_id: "map"  # TF frame_id
+    global_frame_id: "earth"  # TF earth frame_id
+    child_frame_id: "base_link" # TF child_frame_id
+
+# imu_pub
+imu:
+  frame_id: "base_link"
+  # need find actual values
+  linear_acceleration_stdev: 0.0003
+  angular_velocity_stdev: 0.0003490659 // 0.02 degrees
+  orientation_stdev: 1.0
+  magnetic_stdev: 0.0
+
+# local_position
+local_position:
+  frame_id: "map"
+  tf:
+    send: false
+    frame_id: "map"
+    child_frame_id: "base_link"
+    send_fcu: false
+
+# param
+# None, used for FCU params
+
+# rc_io
+# None
+
+# safety_area
+safety_area:
+  p1: {x:  1.0, y:  1.0, z:  1.0}
+  p2: {x: -1.0, y: -1.0, z: -1.0}
+
+# setpoint_accel
+setpoint_accel:
+  send_force: false
+
+# setpoint_attitude
+setpoint_attitude:
+  reverse_thrust: false     # allow reversed thrust
+  use_quaternion: true     # enable PoseStamped topic subscriber
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "target_attitude"
+    rate_limit: 50.0
+
+setpoint_raw:
+  thrust_scaling: 1.0       # used in setpoint_raw attitude callback.
+  # Note: PX4 expects normalized thrust values between 0 and 1, which means that
+  # the scaling needs to be unitary and the inputs should be 0..1 as well.
+
+# setpoint_position
+setpoint_position:
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "target_position"
+    rate_limit: 50.0
+  mav_frame: LOCAL_NED
+
+# setpoint_velocity
+setpoint_velocity:
+  mav_frame: LOCAL_NED
+
+# vfr_hud
+# None
+
+# waypoint
+mission:
+  pull_after_gcs: true  # update mission if gcs updates
+
+# --- mavros extras plugins (same order) ---
+
+# adsb
+# None
+
+# debug_value
+# None
+
+# distance_sensor
+## Currently available orientations:
+#    Check http://wiki.ros.org/mavros/Enumerations
+##
+distance_sensor:
+  hrlv_ez4_pub:
+    id: 0
+    frame_id: "hrlv_ez4_sonar"
+    orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
+    field_of_view: 0.0  # XXX TODO
+    send_tf: true
+    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
+  lidarlite_pub:
+    id: 1
+    frame_id: "lidarlite_laser"
+    orientation: PITCH_270
+    field_of_view: 0.0  # XXX TODO
+    send_tf: true
+    sensor_position: {x:  0.0, y:  0.0, z:  -0.1}
+  sonar_1_sub:
+    subscriber: true
+    id: 2
+    orientation: PITCH_270
+  laser_1_sub:
+    subscriber: true
+    id: 3
+    orientation: PITCH_270
+
+# image_pub
+image:
+  frame_id: "px4flow"
+
+# fake_gps
+fake_gps:
+  # select data source
+  use_mocap: true         # ~mocap/pose
+  mocap_transform: false   # ~mocap/tf instead of pose
+  use_vision: false       # ~vision (pose)
+  # origin (default: Zürich)
+  geo_origin:
+    lat: 47.3667          # latitude [degrees]
+    lon: 8.5500           # longitude [degrees]
+    alt: 408.0            # altitude (height over the WGS-84 ellipsoid) [meters]
+  eph: 2.0
+  epv: 2.0
+  satellites_visible: 5   # virtual number of visible satellites
+  fix_type: 3             # type of GPS fix (default: 3D)
+  tf:
+    listen: false
+    send: false           # send TF?
+    frame_id: "map"       # TF frame_id
+    child_frame_id: "fix" # TF child_frame_id
+    rate_limit: 10.0      # TF rate
+  gps_rate: 5.0           # GPS data publishing rate
+
+# landing_target
+landing_target:
+  listen_lt: false
+  mav_frame: "LOCAL_NED"
+  land_target_type: "VISION_FIDUCIAL"
+  image:
+    width: 640            # [pixels]
+    height: 480
+  camera:
+    fov_x: 2.0071286398   # default: 115 [degrees]
+    fov_y: 2.0071286398
+  tf:
+    send: true
+    listen: false
+    frame_id: "landing_target"
+    child_frame_id: "camera_center"
+    rate_limit: 10.0
+  target_size: {x:  0.3, y:  0.3}
+
+# mocap_pose_estimate
+mocap:
+  # select mocap source
+  use_tf: false   # ~mocap/tf
+  use_pose: true  # ~mocap/pose
+
+# odom
+odometry:
+  in:
+    frame_id: "odom"
+    child_frame_id: "base_link"
+    frame_tf:
+      local_frame: "local_origin_ned"
+      body_frame_orientation: "flu"
+  out:
+    frame_tf:
+      # available: check MAV_FRAME odometry local frames in
+      # https://mavlink.io/en/messages/common.html
+      local_frame: "vision_ned"
+      # available: ned, frd or flu (though only the tf to frd is supported)
+      body_frame_orientation: "frd"
+
+# px4flow
+px4flow:
+  frame_id: "px4flow"
+  ranger_fov: 0.118682      # 6.8 degrees at 5 meters, 31 degrees at 1 meter
+  ranger_min_range: 0.3     # meters
+  ranger_max_range: 5.0     # meters
+
+# vision_pose_estimate
+vision_pose:
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "vision_estimate"
+    rate_limit: 10.0
+
+# vision_speed_estimate
+vision_speed:
+  listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
+  twist_cov: true       # enable listen to twist with covariance topic
+
+# vibration
+vibration:
+  frame_id: "base_link"
+
+# wheel_odometry
+wheel_odometry:
+  count: 2           # number of wheels to compute odometry
+  use_rpm: false     # use wheel's RPM instead of cumulative distance to compute odometry
+  wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
+  wheel1: {x: 0.0, y:  0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
+  send_raw: true              # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
+  send_twist: false           # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
+  frame_id: "map"             # origin frame
+  child_frame_id: "base_link" # body-fixed frame
+  vel_error: 0.1              # wheel velocity measurement error 1-std (m/s)
+  tf:
+    send: true
+    frame_id: "map"
+    child_frame_id: "base_link"
+
+# vim:set ts=2 sw=2 et:

config/px4_pluginlists.yaml

@@ -0,0 +1,15 @@
+plugin_blacklist:
+# common
+- safety_area
+# extras
+- image_pub
+- vibration
+- distance_sensor
+- rangefinder
+- wheel_odometry
+- mission
+- tdr_radio
+- fake_gps
+
+plugin_whitelist: []
+#- 'sys_*'

frames.gv

@@ -1,9 +0,0 @@
-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";
-}

launch/cortex_bridge.launch

@@ -14,6 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		pkg="oscillation_ctrl"
 		type="MoCap_Localization_noTether.py"
 		name="localize_node"
+		launch-prefix="xterm -e"
 	/>
 
 	<!-- Cortex bridge launch  -->
@@ -21,4 +22,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 
 	<!-- MAVROS launch  -->
 	<include file="$(find mavros)/launch/px4.launch"/>
+		<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
 </launch>

launch/klausen_dampen.launch

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

src/LinkState.py

@@ -11,7 +11,6 @@ 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:
@@ -53,6 +52,9 @@ class Main:
 		self.phidot_max   = 3.0
 		self.thetadot_max = 3.0 
 
+		# Vehicle is spawned with yaw offset for convenience, need to deal with that
+		self.yaw_offset = 0.0
+
 		# variables for message gen
 		self.status 					= tethered_status()
 		self.status.drone_id	= 'spiri_with_tether::spiri::base'
@@ -97,18 +99,18 @@ class Main:
 			output = value
 		return output
 
-	def euler_array(self,pose):
+	def euler_array(self,orientation):
 		"""
 		Takes in pose msg object and outputs array of euler angs:
-			q[0] = Roll
+			eul[0] = Roll
-			q[1] = Pitch
+			eul[1] = Pitch
-			q[2] = Yaw
+			eul[2] = Yaw
 		"""
-		q = euler_from_quaternion([pose.orientation.x,
+		eul = euler_from_quaternion([orientation.x,
-															pose.orientation.y,
+															orientation.y,
-															pose.orientation.z,
+															orientation.z,
-															pose.orientation.w])
+															orientation.w])
-		return q
+		return eul
 		
 	# Get link states (drone and pload) and determine angle between them
 	def link_state(self,pub_timer):
@@ -125,8 +127,8 @@ class Main:
 			# P = Position vector
 			drone_P = get_P(self.status.drone_id,reference)
 
-			# Get orientation of drone in euler angles
+			# Get orientation of drone in euler angles to determine yaw offset
-			drone_Eul = self.euler_array(drone_P.link_state.pose)
+			drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
 			
 			# Check if payload is part of simulation
 			if not drone_P.success:
@@ -138,6 +140,9 @@ class Main:
 
 			if not self.has_run == 1:
 				if self.pload == True:
+					# Determine yaw offset
+					self.yaw_offset = drone_Eul[2]	
+			
 					# Get tether length based off initial displacement
 					self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - 
 												pload_P.link_state.pose.position.x)**2 +
@@ -158,6 +163,19 @@ class Main:
 			drone_Py = drone_P.link_state.pose.position.y
 			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]
+
 			if self.pload == True: # If there is payload, determine the variables
 				# Pload
 				pload_Px = pload_P.link_state.pose.position.x
@@ -195,7 +213,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],2))
+			print"Roll:           "+str(round(drone_Eul[0],2)),"\nPitch:         "+str(round(drone_Eul[1],2)),"\nYaw:           "+str(round(drone_Eul[2],4))
 			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))

src/MoCap_Localization_noTether.py

@@ -9,7 +9,7 @@ import time
 import math
 from tf.transformations import *
 from oscillation_ctrl.msg import tethered_status
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, Point
 from std_msgs.msg import Bool
 
 class Main:
@@ -19,8 +19,6 @@ class Main:
 		# rate(s)
 		rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
 
-		self.dt		 = 1.0/rate
-
 		# 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
@@ -41,19 +39,10 @@ class Main:
 		### -*-*-*- END -*-*-*- ###
 
 		# initialize variables
-		#self.buff_pose1  = PoseStamped()
 		self.drone_pose = PoseStamped()
 		self.buff_pose  = PoseStamped()
 
-		# Debugging
+		self.eul = [0.0,0.0,0.0]
-#		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
@@ -71,12 +60,6 @@ class Main:
 
 
 		# publisher(s)
-		#self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
-		#self.pub_wd	 	 = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
-
-		#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)
-
 		### Since there is no tether, we can publish directly to mavros
 		self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
 		
@@ -104,18 +87,17 @@ class Main:
 			output = value
 		return output
 
-	def euler_array(self,pose):
+	def euler_array(self):
 		"""
 		Takes in pose msg object and outputs array of euler angs:
-			q[0] = Roll
+			eul[0] = Roll
-			q[1] = Pitch
+			eul[1] = Pitch
-			q[2] = Yaw
+			eul[2] = Yaw
 		"""
-		q = euler_from_quaternion([pose.orientation.x,
+		self.eul = euler_from_quaternion([self.drone_pose.pose.orientation.x,
-															pose.orientation.y,
+															self.drone_pose.pose.orientation.y,
-															pose.orientation.z,
+															self.drone_pose.pose.orientation.z,
-															pose.orientation.w])
+															self.drone_pose.pose.orientation.w])
-		return q
 
 	def FRD_Transform(self):
 		'''
@@ -123,7 +105,20 @@ class Main:
 		'''
 
 		self.drone_pose = self.buff_pose
-		self.drone_pose.header.frame_id = "map"
+		self.drone_pose.header.frame_id = "/map"
+
+#		self.drone_pose.pose.position.x = 0
+#		self.drone_pose.pose.position.y = 0
+#		self.drone_pose.pose.position.z = 0.5
+
+		#Keep the w same and change x, y, and z as above.
+#		self.drone_pose.pose.orientation.x = 0
+#		self.drone_pose.pose.orientation.y = 0
+#		self.drone_pose.pose.orientation.z = 0
+#		self.drone_pose.pose.orientation.w = 1
+
+		self.euler_array() # get euler angles of orientation for user
+
 #		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
@@ -157,6 +152,9 @@ class Main:
 		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))
+		print "Roll:           " + str(round(self.eul[0]*180/3.14,2))
+		print "Pitch:          " + str(round(self.eul[1]*180/3.14,2))
+		print "Yaw:            " + str(round(self.eul[2]*180/3.14,2))
 
 
 if __name__=="__main__":

src/klausen_control.py

@@ -308,9 +308,6 @@ 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.orientation.x = q[0] 
 		self.quaternion.pose.orientation.y = q[1]
 		self.quaternion.pose.orientation.z = q[2]

src/offb_node.cpp

@@ -2,7 +2,6 @@
  * @file offb_node.cpp
  * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
  * Stack and tested in Gazebo SITL
- * Cesar: Now used for path following on topic \path 
  */
 
 #include <ros/ros.h>

src/path_follow.cpp

@@ -5,8 +5,9 @@
 
 #include <ros/ros.h>
 #include <sensor_msgs/NavSatFix.h>
-#include <offboard_ex/RefSignal.h>
+#include <oscillation_ctrl/RefSignal.h>
-#include <offboard_ex/EulerAngles.h>
+#include <oscillation_ctrl/EulerAngles.h>
+#include <oscillation_ctrl/WaypointTrack.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Vector3.h>
@@ -31,7 +32,7 @@ geometry_msgs::PoseStamped desPose;
 tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent;
 double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd
 geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
-offboard_ex::EulerAngles eulAng;
+oscillation_ctrl::EulerAngles eulAng;
 
 mavros_msgs::AttitudeTarget thrust;
 
@@ -42,12 +43,9 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){
 }
 
 geometry_msgs::Quaternion q_des;
-double alt_des; // Check desired height
-
 // Cb to determine desired pose *Only needed for orientation 
 void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
     desPose = *msg;
-		alt_des = desPose.pose.position.z;
     q_des   = desPose.pose.orientation;
     tf2::convert(q_des,q_cmd);	// Convert msg type to tf2::quaternion type
 }
@@ -83,7 +81,6 @@ void thrust_cb(const mavros_msgs::AttitudeTarget msg){
 	tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat
 }
 
-
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "path_follow");
@@ -116,7 +113,7 @@ int main(int argc, char **argv)
 		("mavros/setpoint_raw/attitude",10);
 
     // Publish attitude cmds in euler angles
-    ros::Publisher euler_pub = nh.advertise<offboard_ex::EulerAngles>
+    ros::Publisher euler_pub = nh.advertise<oscillation_ctrl::EulerAngles>
 		("command/euler_angles",10);
 
     // Service Clients
@@ -126,6 +123,8 @@ int main(int argc, char **argv)
     ("mavros/set_mode");
     ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
 		("mavros/cmd/takeoff");
+		ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
+		("status/waypoint_tracker");
 
     //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
     ros::Rate rate(20.0);
@@ -144,14 +143,20 @@ 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
+		// Retrieve desired waypoints
-		/*std::vector<double> waypoints;		
+		oscillation_ctrl::WaypointTrack wpoint_srv;
-		nh.getParam("sim/waypoints",waypoints);		
+		wpoint_srv.request.query = false;
+		if (waypoint_cl.call(wpoint_srv))
+		{
+			ROS_INFO("Waypoints received");
+		}
 	
-    // Need to send pose commands until throttle has been established
+		// populate desired waypoints - this is only for original hover as 
-    buff_pose.pose.position.x = waypoints(1);
+		// a change of waypoints is handled by ref_signal.py
-    buff_pose.pose.position.y = waypoints(2);
+    buff_pose.pose.position.x = wpoint_srv.response.xd.x;
-    buff_pose.pose.position.z = waypoints(3);*/
+    buff_pose.pose.position.y = wpoint_srv.response.xd.y;
+    buff_pose.pose.position.z = wpoint_srv.response.xd.z;
+		double alt_des = buff_pose.pose.position.z; // Desired height
 
     // Desired mode is offboard
     mavros_msgs::SetMode offb_set_mode;

src/ref_signalGen.py

@@ -8,22 +8,17 @@ import numpy as np
 import time
 import math
 
+from pymavlink import mavutil
 from scipy import signal
 from scipy.integrate import odeint 
 
 from oscillation_ctrl.msg import tethered_status, RefSignal
-
+from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
 from controller_msgs.msg import FlatTarget
-
+from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped 
-from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point 
-
-from geometry_msgs.msg import TwistStamped
 from sensor_msgs.msg import Imu
-
 from mavros_msgs.msg import State
 
-from pymavlink import mavutil
-
 class Main:
 
 	def __init__(self):
@@ -35,6 +30,7 @@ class Main:
 		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()
+		rospy.loginfo(self.tstart)	
 
 		self.dt 			= 1.0/rate
 #		self.dt 			= 0.5
@@ -71,6 +67,33 @@ class Main:
 			if rospy.has_param('sim/tether_length'):
 				self.tetherL	= rospy.get_param('sim/tether_length') # Tether length
 				self.param_exists = True
+			elif rospy.get_time() - self.tstart >= 3.0:
+				self.tetherL	= 0.0
+				break
+		
+# --------------------------------------------------------------------------------#		
+# 																	SUBSCRIBERS																		#
+# --------------------------------------------------------------------------------#
+		# Topic, msg type, and class callback method
+		rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
+		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)
+
+# --------------------------------------------------------------------------------#		
+# 																	PUBLISHERS
+# --------------------------------------------------------------------------------#
+		# Publish desired path to compute attitudes
+		self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
+		# Needed for geometric controller to compute thrust
+		self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
+
+		# timer(s), used to control method loop freq(s) as defined by the rate(s)
+		self.pub_tim  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
+
+# --------------------------------------------------------------------------------#		
+# 														FEEDBACK AND INPUT SHAPING
+# --------------------------------------------------------------------------------#
 
 		# Smooth path variables 
 		self.EPS_F 		= np.zeros(9)    # Final Epsilon/ signal
@@ -79,7 +102,12 @@ class Main:
 		# Constants for smooth path generation
 		self.w_tune		= 3.13 # 3.13 works well? #########################################################################
 		self.epsilon	= 0.7 								# Damping ratio
-		self.wn				= math.sqrt(9.81/self.tetherL)
+		
+		# need exception if we do not have tether:
+		if self.tetherL == 0.0:
+			self.wn = self.w_tune
+		else:
+			self.wn				= math.sqrt(9.81/self.tetherL)
 		self.wd				= self.wn*math.sqrt(1 - self.epsilon**2)
 		self.k4				= 4*self.epsilon*self.w_tune
 		self.k3				= ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4
@@ -100,9 +128,9 @@ class Main:
 		#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.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.des_waypoints = True # True = changing waypoints
 
@@ -111,10 +139,6 @@ class Main:
 		self.y0 = [0, 0, 0, 0]
 		self.z0 = [0, 0, 0, 0]
 
-# --------------------------------------------------------------------------------#		
-# 														FEEDBACK AND INPUT SHAPING
-# --------------------------------------------------------------------------------#
-
 		# Constants for feedback
 		self.Gd = 0.325 
 		self.td = 2*self.Gd*math.pi/self.wd
@@ -148,30 +172,6 @@ class Main:
 		self.sk				=	len(self.SF_delay_x[0])  # from Klausen 2017
 		self.ak 			= np.zeros(len(self.SF_delay_x[0]))
 		self.s_gain 	= 0.0 # Gain found from sigmoid	
-		
-# --------------------------------------------------------------------------------#		
-# 																	SUBSCRIBERS																		#
-# --------------------------------------------------------------------------------#
-		# Topic, msg type, and class callback method
-		rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
-		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',Position, self.waypoints_cb) ### Change to position
-
-# --------------------------------------------------------------------------------#		
-# 																	PUBLISHERS
-# --------------------------------------------------------------------------------#
-		# Publish desired path to compute attitudes
-		self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
-		# Needed for geometric controller to compute thrust
-		self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
-
-		# timer(s), used to control method loop freq(s) as defined by the rate(s)
-		self.pub_tim  = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
-		
-# ------------------------------------ _init_ ends ------------------------------ # 
-
 # --------------------------------------------------------------------------------#		
 # 																CALLBACK FUNCTIONS
 # --------------------------------------------------------------------------------#
@@ -230,14 +230,19 @@ class Main:
 			pass
 
 	def waypoints_srv_cb(self):
-		rospy.wait_for_service('status/waypoint_tracker')
+		rospy.wait_for_service('/status/waypoint_tracker')
 		try:
-			self.xd = self.get_xd(True)
+			resp = self.get_xd(False,self.empty_point)
-#			if self.des_waypoints == True:
+			self.xd = resp.xd
-
 		except ValueError:
 			pass
 										
+#################################################################
+#TODO Will need to add a function that gets a message from  		#
+#			a node which lets refsignal_gen.py know there has been a 	#
+#  		change in xd and therefore runs waypoints_srv_cb again		#
+#################################################################
+
 # --------------------------------------------------------------------------------#		
 # 																			ALGORITHM
 # --------------------------------------------------------------------------------#
@@ -322,9 +327,9 @@ class Main:
 		else:
 			print('No delay')
 
-	# Convolution and input shape filter, and feedback
+	# Convolution of shape filter and trajectory, and feedback
 	def convo(self):
-
+		self.waypoints_srv_cb()
 		# needed for delay function
 		# 1 = determine shapefilter array
 		# 2 = determine theta/phi_fb
@@ -332,9 +337,9 @@ class Main:
 		feedBack = 2
 
 		# SOLVE ODE (get ref pos, vel, accel)
-		self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd[0],))
+		self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
-		self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd[1],))
+		self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
-		self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd[2],))
+		self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
 
 		for i in range(1,len(self.y0)):	
 			self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
@@ -343,7 +348,7 @@ class Main:
 			
 		for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
 
-			self.delay(j,shapeFil) # Determine the delay array
+			self.delay(j,shapeFil) # Determine the delay (shapefilter) array
 
 			if self.SF_idx < len(self.SF_delay_x[0]):
 				self.EPS_I[3*j] 	= self.x[1,j] 
@@ -363,9 +368,21 @@ class Main:
 			# Populate EPS_F buffer with desired change based on feedback
 			self.EPS_F[i] = self.EPS_I[i] + EPS_D[i] #+ EPS_D[i]
 
-#		self.x0 = [self.dr_pos.position.x, self.dr_vel.x, self.dr_acc.x, self.x[1,3]]
+		# Populate msg with epsilon_final
-#		self.y0 = [self.dr_pos.position.y, self.dr_vel.y, self.dr_acc.y, self.y[1,3]] 
+		self.ref_sig.header.stamp = rospy.Time.now()
-#		self.z0 = [self.dr_pos.position.z, self.dr_vel.z, self.dr_acc.z, self.z[1,3]]
+		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.y = self.EPS_F[1]
+		self.ref_sig.position.z = self.EPS_F[2] + 0.5
+
+		self.ref_sig.velocity.x = self.EPS_F[3]
+		self.ref_sig.velocity.y	= self.EPS_F[4]
+		self.ref_sig.velocity.z	= self.EPS_F[5]
+
+		self.ref_sig.acceleration.x	= self.EPS_F[6]
+		self.ref_sig.acceleration.y	= self.EPS_F[7]
+		self.ref_sig.acceleration.z	= self.EPS_F[8]
 
 		self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
 		self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] 
@@ -389,30 +406,7 @@ class Main:
 
 		return EPS_D 
 
-	def publisher(self,pub_tim):
+	def screen_output(self):
-	
-		# Determine final ref signal		
-		self.convo()
-		
-		# Populate msg with epsilon_final
-		self.ref_sig.position.x = self.EPS_F[0]
-		self.ref_sig.position.y = self.EPS_F[1]
-
-		self.ref_sig.velocity.x = self.EPS_F[3]
-		self.ref_sig.velocity.y	= self.EPS_F[4]
-		self.ref_sig.velocity.z	= self.EPS_F[5]
-
-		self.ref_sig.acceleration.x	= self.EPS_F[6]
-		self.ref_sig.acceleration.y	= self.EPS_F[7]
-		self.ref_sig.acceleration.z	= self.EPS_F[8]
-
-		# Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
-		self.ref_sig.type_mask = 2
-
-		# Publish command
-		self.ref_sig.header.stamp = rospy.Time.now()
-		self.pub_path.publish(self.ref_sig)
-		self.pub_ref.publish(self.ref_sig)
 
 		# Feedback to user
 		rospy.loginfo(' Var |   x   |   y   |   z   ')
@@ -421,6 +415,23 @@ class Main:
 		rospy.loginfo('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
 		rospy.loginfo('_______________________')
 
+	def publisher(self,pub_tim):
+	
+		# Determine final ref signal		
+		self.convo()
+
+		# Publish reference signal
+		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('_______________________')
+
+		
 if __name__=="__main__":
 
 	# Initiate ROS node

src/square.py

@@ -5,7 +5,7 @@
 ### Script to generate set points which form a square with 2m side lengths
 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:
@@ -13,11 +13,11 @@ class Main:
 	def __init__(self):
 
 		# variable(s)
-		self.Point 	= PoseStamped()
+		self.Point 	= Point()
 		# Init x, y, & z coordinates
-		self.Point.pose.position.x	= 0
+		self.Point.x	= 0
-		self.Point.pose.position.y	= 0
+		self.Point.y	= 0
-		self.Point.pose.position.z	= 3.5
+		self.Point.z	= 3.5
 
 		self.xarray = [1,2,2,2,1,0,0]
 		self.yarray = [0,0,1,2,2,2,1]
@@ -30,7 +30,7 @@ class Main:
 		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) 
 
 		# publisher(s), with specified topic, message type, and queue_size
-		self.pub_square = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) 	 	
+		self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) 	 	
 
 		# rate(s)
 		pub_rate = 1 # rate for the publisher method, specified in Hz
@@ -49,28 +49,28 @@ class Main:
 		else:
 			# Check if i is too large. loop back to first point
 			if self.i >= len(self.xarray):
-				self.Point.pose.position.x = 0
+				self.Point.x = 0
-				self.Point.pose.position.y = 0
+				self.Point.y = 0
 
 			else:
-				self.Point.pose.position.x = self.xarray[self.i]
+				self.Point.x = self.xarray[self.i]
-				self.Point.pose.position.y = self.yarray[self.i]
+				self.Point.y = self.yarray[self.i]
 				
 				rospy.loginfo("Sending [Point x] %d [Point y] %d",
-											 self.Point.pose.position.x, self.Point.pose.position.y) 	
+											 self.Point.x, self.Point.y) 	
 				
 			# Published desired msgs
-			self.Point.header.stamp = rospy.Time.now()
+		#	self.Point.header.stamp = rospy.Time.now()
 			self.pub_square.publish(self.Point)
 			self.j += 1
 			self.i = self.j // self.buffer
 
 #		self.Point.header.stamp = rospy.Time.now()
-#		self.Point.pose.position.x = self.xarray[self.i]
+#		self.Point.x = self.xarray[self.i]
-#		self.Point.pose.position.y = self.yarray[self.i]
+#		self.Point.y = self.yarray[self.i]
 		
 #		rospy.loginfo("Sending [Point x] %d [Point y] %d",
-#									 self.Point.pose.position.x, self.Point.pose.position.y) 	
+#									 self.Point.x, self.Point.y) 	
 			
 		# Published desired msgs
 #		self.pub_square.publish(self.Point)

src/step.py

@@ -14,9 +14,9 @@ class Main:
 		# variable(s)
 		self.Point 	= Point()
 		# Init x, y, & z coordinates
-		self.Point.pose.position.x	= 1
+		self.Point.x	= 1
-		self.Point.pose.position.y	= 0
+		self.Point.y	= 0
-		self.Point.pose.position.z	= 4.0
+		self.Point.z	= 4.0
 		
 		self.bool = False
 
@@ -41,13 +41,13 @@ class Main:
 		if self.bool == False: 		
 			rospy.loginfo('Waiting...')
 		else:
-			self.Point.header.stamp = rospy.Time.now()
+			#self.Point.header.stamp = rospy.Time.now()
 			
 			# Published desired msgs
 			self.pub_step.publish(self.Point)
 
 			rospy.loginfo("Sending [Point x] %d [Point y] %d",
-										 self.Point.pose.position.x, self.Point.pose.position.y) 	
+										 self.Point.x, self.Point.y) 	
 
 if __name__=="__main__":
 

src/wpoint_tracker.py

@@ -36,6 +36,7 @@ class Main:
 				self.xd.x = 0.0
 				self.xd.y = 0.0
 				self.xd.z = 5.0
+				break
 
 		# service(s)
 
@@ -47,7 +48,12 @@ class Main:
 
 
 	def waypoint_relay(self,req):
-		self.resp.xd = self.xd
+		#TODO Need to add check to make sure query is boolean
+		if req.query:
+			self.xd = req.xd
+			self.resp.xd = self.xd
+		else:
+			self.resp.xd = self.xd
 		return self.resp				
 
 	# Change desired position if there is a change
@@ -64,6 +70,8 @@ if __name__=="__main__":
 	try:
 		obj = Main() # create class object
 		s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)
+		rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z)
+			
 		rospy.spin() # loop until shutdown signal
 
 	except rospy.ROSInterruptException:

srv/WaypointTrack.srv

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