From 309a33c08d8de6096c9bccdf72c9d78561b76202 Mon Sep 17 00:00:00 2001 From: "cesar.alejnadro" Date: Mon, 18 Apr 2022 10:10:17 -0300 Subject: [PATCH] Merging branches --- CMakeLists.txt | 3 ++ README.md | 23 ++++++++-- launch/klausen_dampen.launch | 1 + package.xml | 16 ++++++- src/LinkState.py | 2 +- src/klausen_control.py | 2 +- src/path_follow.cpp | 2 +- src/perform_test.py | 20 ++++++--- src/ref_signalGen.py | 4 +- src/waypoint_determine.py | 83 ++++++++++++++++++++++++++++++++++++ 10 files changed, 140 insertions(+), 16 deletions(-) create mode 100755 src/waypoint_determine.py diff --git a/CMakeLists.txt b/CMakeLists.txt index cbdd0e2..50ff486 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation message_runtime roscpp + controller_msgs ) ## System dependencies are found with CMake's conventions @@ -75,6 +76,8 @@ generate_messages( DEPENDENCIES std_msgs geometry_msgs + sensor_msgs + controller_msgs ) ################################################ diff --git a/README.md b/README.md index 8867c58..cc595d9 100644 --- a/README.md +++ b/README.md @@ -104,7 +104,7 @@ Initilize rosdep: ### ROMFS/PX4FMU_COMMON - copy (or add) files in _oscillation_ctrl/airframes_ to _PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes_ cp -R ~catkin_ws/src/oscillation_ctrl/airframes/* PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes -- add file name to _CmakeLists.txt_ in same 'airframe' folder (with number) +- add model names to _CmakeLists.txt_ in same 'airframe' folder (with number... 4000_spiri and 4001_spiri_with_tether) - add airframe name in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ (no number!) ### LAUNCH FILES - copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch' @@ -122,9 +122,8 @@ In catkin_ws (or any working directory) add to devel/setup.bash: ## JINJA TETHER FILE - _spiri_with_tether.sdf.jinja_ can be altered to create desired tether model -- changes need to be made in px4 directory and will only take effect and -"make px4_sitl gazebo" - - can do DONT_RUN=1 make px4_sitl gazebo to avoid starting px4 shell and gazebo +- changes need to be made in px4 directory and will only take effect after running: "make px4_sitl gazebo" + - can do "DONT_RUN=1 make px4_sitl gazebo" to avoid starting px4 shell and gazebo - First two elements can be changed to tweak tether parameters - number_elements: number of segments tether will be composed of - tl: segment length (should be no shorter than 0.3 meters) @@ -176,4 +175,20 @@ In catkin_ws (or any working directory) add to devel/setup.bash: /command/bodyrate_command /mavros/state +## Launching simulation +- To launch a simulation, run the following command: + roslaunch oscillation_ctrl klausen_dampen.launch +- This simulation is set to have a Spiri Mu hover at an alitude of 5 m. +- The launch file itself has two usable arguments: +- __model:__ + spiri_with_tether # default + spiri + headless_spiri_with_tether # headless mode: launches with no Gazebo GUI + headless_spiri +-__test:__ + none # default + step # step input - default is 5 m + square # square trajectory +- To run the simulation with a tethered payload headless mode and perform a step test: + roslaunch oscillation_ctrl klausen_dampen.launch model:=headless_spiri_with_tether test:=step diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index a7271a5..0e8ff7c 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -56,6 +56,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="perform_test.py" name="test_node" + launch-prefix="xterm -e" /> diff --git a/package.xml b/package.xml index 9b73b92..3b32055 100644 --- a/package.xml +++ b/package.xml @@ -21,16 +21,30 @@ roscpp roscpp - geometry_msgs + rospy + rospy + + geometry_msgs geometry_msgs + sensor_msgs + sensor_msgs + std_msgs std_msgs + controller_msgs + controller_msgs + message_generation message_runtime message_runtime + python-numpy + python-scipy + python-pymavlink + + diff --git a/src/LinkState.py b/src/LinkState.py index 3a49cbf..040e4c1 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -205,7 +205,7 @@ class Main: def user_feedback(self,gui_timer): # Print and save results - print "\n" + #print "\n" rospy.loginfo("") print"Roll: "+str(round(self.drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(self.drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(self.drone_Eul[2]*180/3.14,2)) print "drone pos.x: " + str(round(self.drone_Px,2)) diff --git a/src/klausen_control.py b/src/klausen_control.py index 769d695..c32e78d 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -52,7 +52,7 @@ class Main: self.EulerAng = [0,0,0] # Will find the euler angles, and then convert to q # Drone var - self.has_run = 0 # Bool to keep track of first run instance + self.has_run = 0 # Bool to keep track of first run instance # Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI self.PHI = np.array([[0,0],[0,0]]) diff --git a/src/path_follow.cpp b/src/path_follow.cpp index 49078d2..5f04a7a 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -229,7 +229,7 @@ int main(int argc, char **argv) attitude.thrust = thrust.thrust; // Determine which messages to send - if(ros::Time::now() - tkoff_req > ros::Duration(30.0) && takeoff){ + if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){ attitude.orientation = q_des; attitude.header.stamp = ros::Time::now(); att_targ.publish(attitude); diff --git a/src/perform_test.py b/src/perform_test.py index a509a02..0424282 100755 --- a/src/perform_test.py +++ b/src/perform_test.py @@ -1,12 +1,12 @@ #!/usr/bin/env python2.7 - ### Cesar Rodriguez June 2021 -### Script to generate set points which form a square with 2m side lengths +### Runs either a step test or a square trajectory to assess controller performance import rospy, tf import time from geometry_msgs.msg import Point from std_msgs.msg import Bool +from oscillation_ctrl.srv import WaypointTrack class Main: # class method used to initialize variables once when the program starts @@ -18,18 +18,23 @@ class Main: # variable(s) self.Point = Point() # Point which will actually be published - self.xd = Point() # buffer point to determine current location + self.xd = Point() # buffer point to determine current location # Init x, y, & z coordinates self.Point.x = 0 self.Point.y = 0 self.Point.z = 3.5 + # Test parameters + # Square self.xarray = [1,2,2,2,1,0,0] self.yarray = [0,0,1,2,2,2,1] self.i = 0 self.j = 0 self.buffer = 10 + + # Step + self.step_size = 5.0 # meters self.bool = False self.param_exists = False # check in case param does not exist @@ -37,10 +42,12 @@ class Main: if rospy.has_param('status/test_type'): self.test = rospy.get_param('status/test_type') self.param_exists = True - elif rospy.get_time() - self.tstart >= 3.0 or rospy.get_param('status/test_type') == 'none': self.test = 'none' break + + # get waypoints + self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) # subscriber(s), with specified topic, message type, and class callback method rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) @@ -57,7 +64,7 @@ class Main: def wait_cb(self,data): self.bool = data - def waypoints_srv_cb(self): + def waypoints_client(self): rospy.wait_for_service('/status/waypoint_tracker') try: resp = self.get_xd(False,self.xd) @@ -81,7 +88,7 @@ class Main: def step(self): # Published desired msgs - self.Point.x = 5 # STEP (meters) + self.Point.x = self.step_size # STEP (meters) def user_feedback(self): rospy.loginfo("Sending [Point x] %d [Point y] %d", @@ -97,6 +104,7 @@ class Main: def pub(self,pub_timer): if self.bool == False: rospy.loginfo('Waiting...') + self.waypoints_client() # get waypoints while waiting elif self.bool == False and self.test == 'none': rospy.loginfo('NO TEST WILL BE PERFORMED') else: diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 6235967..631242a 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -91,7 +91,7 @@ class Main: self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 3.5 # 3.13 works well? ######################################################################### + self.w_tune = 3.13 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: @@ -108,7 +108,7 @@ class Main: # For saturation: self.jmax = [3, 3, 1] - self.amax = [1.5, 1.5, 1] + self.amax = [2, 2, 1] self.vmax = [3, 3, 1] self.max = [0, 3, 1.5, 3] #[0, 3, 1.5, 3] # create the array: [vmax; amax; jmax] diff --git a/src/waypoint_determine.py b/src/waypoint_determine.py new file mode 100755 index 0000000..70029d9 --- /dev/null +++ b/src/waypoint_determine.py @@ -0,0 +1,83 @@ +#!/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 +import math +from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest +from sensor_msgs.msg import NavSatFix + +class Waypoint: + def __init__(self,latitude,longitude): + self.lat = latitude + self.lon = longitude + +class Main: + def __init__(self): + self.request = WaypointTrackRequest() + self.request.query = True # set to true to set new waypoints + self.get_xd = WaypointTrackRequest() # need to get desired height + self.has_run = False + self.waypoints = [] + self.R = 6371e3 # Earth radius in meters + self.global_home = NavSatFix() + + # service(s) + self.waypoint_cl = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) + + # subscribers + rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.globalPos_cb) + + def globalPos_cb(self,msg): + if not self.has_run: # Will not run after global home has been set + if msg.latitude != 0.0 and msg.longitude != 0.0: + self.gl_home = Waypoint(msg.latitude,msg.longitude) # Save global home + self.has_run = True + + def run(self): + self.wp_input() + self.determine_xd(self.wp) + self.set_waypoint() + + def wp_input(self): + # testing: + self.lat = input('Enter lat: ') + self.lon = input('Enter lon: ') + + self.lat = 44.6480876 + self.lon = -63.5782662 + self.wp = Waypoint(self.lat,self.lon) + + def determine_xd(self,wp): + # https://en.wikipedia.org/wiki/Equirectangular_projection + self.request.xd.x = self.R*(wp.lon - self.gl_home.lon)*(math.pi/180)*math.cos((wp.lat + self.gl_home.lat)*(math.pi/360)) + self.request.xd.y = self.R*(wp.lat - self.gl_home.lat)*(math.pi/180) + """ + 360 in xd.x because (wp.lat + self.gl_home.lat)/2 + """ + + def set_waypoint(self): + rospy.wait_for_service('/status/waypoint_tracker') + try: + resp = self.waypoint_cl(self.get_xd) # get xd.z + self.request.xd.z = resp.xd.z + self.waypoint_cl(self.request) + rospy.loginfo(self.waypoint_cl(self.get_xd)) # want to return value to give feedback to user? + except rospy.ServiceException as e: + print('Service call failed: %s'%e) + + +if __name__=="__main__": + + # Initiate ROS node + rospy.init_node('waypoint_client',anonymous=True) + try: + service = Main() # create class object + service.run() + rospy.spin() # loop until shutdown signal + + except rospy.ROSInterruptException: + pass