Merging branches

This commit is contained in:
cesar.alejnadro 2022-04-18 10:10:17 -03:00
parent f943b8db92
commit 309a33c08d
10 changed files with 140 additions and 16 deletions

View File

@ -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
)
################################################

View File

@ -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

View File

@ -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">

View File

@ -21,16 +21,30 @@
<build_depend>roscpp</build_depend>
<exec_depend>roscpp</exec_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 -->

View File

@ -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))

View File

@ -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);

View File

@ -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
@ -25,11 +25,16 @@ class Main:
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,11 +42,13 @@ 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:

View File

@ -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]

83
src/waypoint_determine.py Executable file
View File

@ -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