forked from cesar.alejandro/oscillation_ctrl
Merging branches
This commit is contained in:
parent
f943b8db92
commit
309a33c08d
|
|
@ -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
|
||||
)
|
||||
|
||||
################################################
|
||||
|
|
|
|||
23
README.md
23
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
|
||||
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
/>
|
||||
</group>
|
||||
<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
|
||||
|
|
|
|||
16
package.xml
16
package.xml
|
|
@ -21,16 +21,30 @@
|
|||
<build_depend>roscpp</build_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
<build_depend>controller_msgs</build_depend>
|
||||
<exec_depend>controller_msgs</exec_depend>
|
||||
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>message_runtime</build_depend>
|
||||
<exec_depend>message_runtime</exec_depend>
|
||||
|
||||
<exec_depend>python-numpy</exec_depend>
|
||||
<exec_depend>python-scipy</exec_depend>
|
||||
<exec_depend>python-pymavlink</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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]])
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Loading…
Reference in New Issue