From 469accc8dcc98680d6499569d2a4ff45fb1d9036 Mon Sep 17 00:00:00 2001 From: cesar Date: Thu, 18 Aug 2022 15:29:26 -0300 Subject: [PATCH] More improvements to controller --- .gitignore | 3 + CMakeLists.txt | 63 ++------ config/Ctrl_param.yaml | 7 +- config/gazebo_config.yaml | 4 + config/noCtrl_param.yaml | 7 +- launch/oscillation_damp.launch | 13 +- launch/takeoff_noCtrl.launch | 8 +- msg/TetheredStatus.msg | 4 +- px4_launch/spiri_with_tether.launch | 2 +- src/LinkState.py | 6 +- src/gazebo_setMasses.py | 65 +++++++++ src/klausen_control.py | 219 ++++++++++++++++++---------- src/offb_node.cpp | 23 +-- src/path_follow.cpp | 74 ++++++---- src/perform_test.py | 23 ++- src/ref_signalGen.py | 148 ++++++++++--------- src/wpoint_tracker.py | 10 +- 17 files changed, 417 insertions(+), 262 deletions(-) create mode 100644 config/gazebo_config.yaml create mode 100755 src/gazebo_setMasses.py diff --git a/.gitignore b/.gitignore index 09c6835..065dea8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +config/mocap_* launch/cortex_bridge.launch launch/debug.launch launch/klausen_dampen.launch @@ -7,10 +8,12 @@ src/killswitch_client.py src/land_client.py src/MoCap_*.py src/Mocap_*.py +src/mocap_* src/segmented_tether.py src/segmented_tether_fast.py msg/Marker.msg msg/Markers.msg *.rviz +CMakeLists.txt setup.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 476b763..1d72db3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,39 +15,10 @@ find_package(catkin REQUIRED COMPONENTS roscpp ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - ################################################ ## Declare ROS messages, services and actions ## ################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - ## Generate messages in the 'msg' folder add_message_files( FILES @@ -78,26 +49,6 @@ generate_messages( sensor_msgs ) -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - ################################### ## catkin specific configuration ## ################################### @@ -112,7 +63,7 @@ catkin_package( # LIBRARIES oscillation_ctrl CATKIN_DEPENDS CATKIN_DEPENDS roscpp mavros geometry_msgs message_runtime -# DEPENDS system_lib + ) ########### @@ -138,6 +89,18 @@ target_link_libraries(pathFollow_node ${catkin_LIBRARIES}) add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) +#add_executable(mocap_offb_node src/mocap_offb_node.cpp) + +#target_link_libraries(mocap_offb_node ${catkin_LIBRARIES}) + +#add_dependencies(mocap_offb_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) + +#add_executable(mocap_pathFollow_node src/mocap_path_follow.cpp) + +#target_link_libraries(mocap_pathFollow_node ${catkin_LIBRARIES}) + +#add_dependencies(mocap_pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS}) + ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/oscillation_ctrl.cpp diff --git a/config/Ctrl_param.yaml b/config/Ctrl_param.yaml index 30358c6..c2990d6 100644 --- a/config/Ctrl_param.yaml +++ b/config/Ctrl_param.yaml @@ -1,5 +1,8 @@ # Ros param when using Klausen Ctrl +wait_time: 30 +#drone_mass: 0.5841 +drone_mass: 1.437 +pload_mass: 0.50 +use_ctrl: false -wait: 40 -waypoints: {x: 0.0, y: 0.0, z: 5.0} diff --git a/config/gazebo_config.yaml b/config/gazebo_config.yaml new file mode 100644 index 0000000..4868e62 --- /dev/null +++ b/config/gazebo_config.yaml @@ -0,0 +1,4 @@ +# Ros param when not using Klausen Ctrl +waypoints: {x: 0.0, y: 0.0, z: 1.5} +square_x: [0.5,1,1,1,0.5,0,0] +square_y: [0,0,0.5,1,1,1,0.5] diff --git a/config/noCtrl_param.yaml b/config/noCtrl_param.yaml index c317ad3..4d81a8c 100644 --- a/config/noCtrl_param.yaml +++ b/config/noCtrl_param.yaml @@ -1,4 +1,7 @@ # Ros param when not using Klausen Ctrl +wait_time: 40 +drone_mass: 0.5841 +#drone_mass: 1.437 +pload_mass: 0.10 + -wait: 55 -waypoints: {x: 0.0, y: 0.0, z: 5.0} diff --git a/launch/oscillation_damp.launch b/launch/oscillation_damp.launch index 0e5662d..40aaaf2 100644 --- a/launch/oscillation_damp.launch +++ b/launch/oscillation_damp.launch @@ -5,17 +5,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo - - - - - + + + + - - - - - + + + + + diff --git a/msg/TetheredStatus.msg b/msg/TetheredStatus.msg index 0a05e8c..2e8c0cc 100644 --- a/msg/TetheredStatus.msg +++ b/msg/TetheredStatus.msg @@ -1,6 +1,6 @@ -# Reports position of drone, payload, and the roll angle between them +# Reports position of drone, payload std_msgs/Header header # Header -std_msgs/Bool bool # Boolean, True = Payload +bool pload # Boolean, True = Payload geometry_msgs/Pose drone_pos # Drone pose geometry_msgs/Pose pload_pos # Payload pose diff --git a/px4_launch/spiri_with_tether.launch b/px4_launch/spiri_with_tether.launch index d9875d9..4da8c16 100644 --- a/px4_launch/spiri_with_tether.launch +++ b/px4_launch/spiri_with_tether.launch @@ -45,7 +45,7 @@ - + diff --git a/src/LinkState.py b/src/LinkState.py index 5ea046e..b64f30f 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -19,7 +19,7 @@ class Main: # rate(s) rate = 60 # rate for the publisher method, specified in Hz -- 20 Hz - self.dt = 1.0/rate + self.dt = 1.0/rate rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru @@ -30,7 +30,7 @@ class Main: # How long should we wait before before starting test self.param_exists = False while self.param_exists == False: - if rospy.has_param('sim/wait'): + if rospy.has_param('wait_time'): self.wait = rospy.get_param('sim/wait') # wait time self.param_exists = True elif rospy.get_time() - self.tstart >= 3.0: @@ -143,7 +143,7 @@ class Main: pload_P.link_state.pose.position.y)**2 + (drone_P.link_state.pose.position.z - pload_P.link_state.pose.position.z)**2) - rospy.set_param('sim/tether_length',self.tetherL) + rospy.set_param('status/tether_length',self.tetherL) else: self.tetherL = 0 diff --git a/src/gazebo_setMasses.py b/src/gazebo_setMasses.py new file mode 100755 index 0000000..585bdec --- /dev/null +++ b/src/gazebo_setMasses.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez Jul 2022 +### Sets mass of links in gazebo to desired values + +import rospy +from gazebo_msgs.srv import SetLinkProperties + + +class Main: + def __init__(self): + 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() + + # Constants + self.param_exists = False + while self.param_exists == False: + if rospy.has_param('drone_mass'): + self.drone_m = rospy.get_param('drone_mass') # wait time + self.param_exists = True + elif rospy.has_param('sim/drone_mass'): + self.drone_m = rospy.get_param('sim/drone_mass') # wait time + self.param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + self.drone_m = 1.0 + rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE') + break + + self.param_exists = False + while self.param_exists == False: + if rospy.has_param('pload_mass'): + self.pl_m = rospy.get_param('pload_mass') # wait time + self.param_exists = True + elif rospy.has_param('sim/pload_mass'): + self.pl_m = rospy.get_param('sim/pload_mass') # wait time + self.param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + self.pl_m = 0.0 + rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE') + break + + self.drone_ids = ['spiri_with_tether::spiri::base', 'spiri::base'] + self.pload_id = 'spiri_with_tether::mass::payload' + self.service = '/gazebo/set_link_properties' + + self.set_mass = rospy.ServiceProxy(self.service,SetLinkProperties) + + for name in self.drone_ids: + self.set_drone_mass = self.set_mass(link_name=name,mass=self.drone_m) + if self.set_drone_mass.success: + rospy.loginfo('Set "%s" mass to: %.2f kg', name,self.drone_m) + break + + self.set_pload_mass = self.set_mass(link_name=self.pload_id,mass=self.pl_m) + if self.set_pload_mass.success: + rospy.loginfo('Set "%s" mass to: %.2f kg', self.pload_id,self.pl_m) + + +if __name__=="__main__": + + # Initiate ROS node + rospy.init_node('SetMassNode',anonymous=False) + Main() # create class object + diff --git a/src/klausen_control.py b/src/klausen_control.py index 5f4b44a..0868484 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -11,21 +11,28 @@ import rospy, tf import numpy as np import time import math +import rosservice from tf.transformations import * from scipy.integrate import odeint -from oscillation_ctrl.msg import RefSignal, LoadAngles +from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus from oscillation_ctrl.srv import WaypointTrack from geometry_msgs.msg import Pose, Point, TwistStamped, PoseStamped from sensor_msgs.msg import Imu from mavros_msgs.msg import AttitudeTarget +class DesiredPoint(): + def __init__(self,x,y,z): + self.x = x + self.y = y + self.z = z + class Main: def __init__(self): # rate(s) - rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz + rate = 25 # rate for the publisher method, specified in Hz -- 50 Hz # initialize variables @@ -54,7 +61,7 @@ class Main: self.empty_point = Point() # Needed to query waypoint_server # 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]]) @@ -65,19 +72,13 @@ class Main: # Tether var - Check if current method is used # Get tether length self.param_exists = False - while self.param_exists == False: # Need to wait until param is ready - if rospy.has_param('sim/tether_length'): - self.tetherL = rospy.get_param('sim/tether_length') # Tether length - self.param_exists = True - self.tether = True - elif rospy.get_time() - self.tstart >= 10.0: - self.tetherL = 0.0 - rospy.loginfo('waiting for tether length') - break + self.tetherL = self.get_tether() + self.tether = True if self.tetherL > 0.01 else False - # Tuning gains - self.K1 = np.identity(3) - self.K2 = np.identity(5) + # Retrieve drone and payload masses from config file + [self.drone_m, self.pl_m] = self.get_masses() + rospy.loginfo('DRONE MASS: %.2f',self.drone_m) + self.tot_m = self.drone_m + self.pl_m # Values which require updates. *_p = prior self.z1_p = np.zeros([3,1]) @@ -90,33 +91,42 @@ class Main: # Error terms self.z1 = np.zeros([3,1]) # dr_pos - ref_sig_pos self.z2 = np.zeros([5,1]) # [vx;vy;vz;thetadot;phidot] - alpha + + # Tuning gains + self.K1 = np.identity(3) + self.K2 = np.identity(5) + self.tune = 0.1 # Tuning parameter + self.dist = np.array([0,0,0,0.1,0.1]) # Wind disturbance # np.array([0,0,0,0.01,0.01]) + # Gain terms + self.Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) + self.Kd = self.tot_m*self.K1 + self.tune*self.K2[:3,:3] + self.Ki = self.tune*self.K1 - # Constants - self.drone_m = 1.437 - self.pl_m = 0.5 - self.tot_m = self.drone_m + self.pl_m - - self.tune = 1 # Tuning parameter - self.dist = np.array([0,0,0,0.01,0.01]) # Wind disturbance - # PD Thrust Controller terms # gains for thrust PD Controller - #self.Kp = 2.7 + #self.Kp = 3.0 #self.Kd = 3 - self.Kp = 3.0 - self.Kd = 3 - self.max_a = 14.2 + self.Kp_thrust = 1.5 + self.Kd_thrust = 1 + self.max_a = 14.2 #TODO self.max_t = self.tot_m*self.max_a self.R = np.empty([3,3]) # rotation matrix self.e3 = np.array([[0],[0],[1]]) - self.thrust_offset = 1.0 # There was found to be a constant offset 0.7 + self.gravity = np.array([[0],[0],[9.81]]) + + self.thrust_offset = 0.0 #1.0 # There was found to be a constant offset 0.7 + + # Get scaling thrust factor, kf + self.kf = self.get_kf() + + self.service_list = rosservice.get_service_list() # --------------------------------------------------------------------------------# # SUBSCRIBERS # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/reference/path', RefSignal, self.refsig_cb) - #rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) + # rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -130,6 +140,58 @@ class Main: # timer(s), used to control method loop freq(s) as defined by the rate(s) self.pub_time = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) #f this was 5.0 rate before +# --------------------------------------------------------------------------------# +# SETUP PARAM METHODS +# --------------------------------------------------------------------------------# + + def get_tether(self): + """ Get tether length based on set parameters""" + param_exists = False + while param_exists == False: + if rospy.has_param('status/tether_length'): + tether_length = rospy.get_param('status/tether_length') # Tether length + param_exists = True + elif rospy.get_time() - self.tstart >= 5.0: + tether_length = 0.0 + break + return tether_length + + def get_masses(self): + """ Use parameters to retrieve drone and payload mass set in config file and sets total mass of bodies""" + # Constants + param_exists = False # reset to False to get body masses + while param_exists == False: + if rospy.has_param('status/drone_mass'): + drone_m = rospy.get_param('status/drone_mass') # wait time + param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + drone_m = 0.5841 + rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE') + break + if self.tether: + param_exists = False + while param_exists == False: + if rospy.has_param('status/pload_mass'): + pl_m = rospy.get_param('status/pload_mass') # wait time + param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + pl_m = 0.0 + rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE') + break + else: + pl_m = 0.0 + + rospy.loginfo('PLOAD MASS: %.2f',pl_m) + return [drone_m, pl_m] + + def get_kf(self): + if rospy.has_param('mocap/hover_throttle'): + hover_throttle = rospy.get_param('mocap/hover_throttle') + else: + hover_throttle = (self.tot_m*9.81 + 9.57)/34.84 # linear scaling depending on dependent on mass + kf = hover_throttle/(self.tot_m*9.81) + return kf + # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -138,20 +200,16 @@ class Main: def loadAngles_cb(self,msg): try: self.load_angles = msg - # Populate self.PHI self.PHI = np.array([[self.load_angles.theta,self.load_angles.thetadot],[self.load_angles.phi,self.load_angles.phidot]]) - except ValueError: pass - # Callback drone pose def dronePos_cb(self,msg): try: self.dr_pos = msg.pose - #self.dr_pos = msg.drone_pos - + # self.dr_pos = msg.drone_pos except ValueError: pass @@ -177,7 +235,8 @@ class Main: try: self.path_pos = np.array([[msg.position.x],[msg.position.y],[msg.position.z]]) self.path_vel = np.array([[msg.velocity.x],[msg.velocity.y],[msg.velocity.z]]) - self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO + self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z]]) #TODO + # self.path_acc = np.array([[msg.acceleration.x],[msg.acceleration.y],[msg.acceleration.z + 9.81]]) #TODO except ValueError: pass @@ -190,12 +249,15 @@ class Main: rospy.loginfo_once('NO TETHER DETECTED') def waypoints_srv_cb(self): - rospy.wait_for_service('/status/waypoint_tracker') - try: - resp = self.get_xd(False,self.empty_point) - self.xd = resp.xd - except ValueError: - pass + if '/status/waypoint_tracker' in self.service_list: + rospy.wait_for_service('/status/waypoint_tracker') + try: + resp = self.get_xd(False,self.empty_point) + self.xd = resp.xd + except ValueError: + pass + else: + self.xd = DesiredPoint(0.0,0.0,1.5) # ---------------------------------ODE SOLVER-------------------------------------# def statespace(self,y,t,Ka,Kb,Kc): @@ -253,29 +315,29 @@ class Main: # thrust as per Geometric Tracking Control of a Quadrotor UAV on SE(3) # Taeyoung Lee, Melvin Leok, and N. Harris McClamroch self.waypoints_srv_cb() - self.error = np.array([[self.dr_pos.position.x - self.xd.x], - [self.dr_pos.position.y - self.xd.y], - [self.dr_pos.position.z - self.xd.z - self.thrust_offset]]) + self.R = self.quaternion_rotation_matrix() + self.error = np.array([ [self.path_pos[0] - self.dr_pos.position.x], + [self.path_pos[1] - self.dr_pos.position.y], + [self.path_pos[2] - self.dr_pos.position.z]]).reshape(3,1) - self.error_vel = self.dr_vel - self.path_vel + self.error_vel = self.path_vel - self.R.dot(self.dr_vel) - self.R = self.quaternion_rotation_matrix() # determine Rotation Matrix + # determine Rotation Matrix self.R_e3 = np.array([[self.R.T[2][0]],[self.R.T[2][1]],[self.R.T[2][2]]]) - - #thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t - #thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t - #thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t - thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t - #thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2] - thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) - #thrust = thrust_vector[2] + # test which one is better: + # thrust_vector = (9.81*self.tot_m*self.e3 + self.Kp_thrust*self.error + self.Kd_thrust*self.error_vel - self.tot_m*self.path_acc)*self.kf + # thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2] + ### OR: + thrust_vector = (9.81*self.tot_m + self.Kp_thrust*self.error[2] + self.Kd_thrust*self.error_vel[2] - self.tot_m*self.path_acc[2])*self.kf + thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1])) ##### # Value needs to be between 0 - 1.0 self.att_targ.thrust = max(0.0,min(thrust,1.0)) def determine_q(self): + """ Determine attitude commands based on feedback and feedforward methods""" # Populate position vector and gamma (g). g is state space vector: [px,py,pz,theta,phi] p = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z]]) g = np.array([[self.dr_pos.position.x],[self.dr_pos.position.y],[self.dr_pos.position.z],[self.load_angles.theta],[self.load_angles.phi]]) @@ -287,12 +349,6 @@ class Main: s_theta = math.sin(self.load_angles.theta) s_phi = math.sin(self.load_angles.phi) - # Check for tether - if L <= 0.01: - self.tether = False - else: - self.tether = True - # Check if tether was correctly detected self.tether_check() @@ -311,7 +367,7 @@ class Main: G = [[0],[0],[-9.81*self.tot_m],[L*9.81*self.pl_m*c_theta*s_phi],[L*9.81*self.pl_m*c_phi*s_theta]] - H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]] + H = [[1,0,0,0,0],[0,1,0,0,0],[0,0,1,0,0]] D = np.diag(self.dist) # Already in array format @@ -355,6 +411,7 @@ class Main: # Determine a_1:3 self.alpha[:3] = self.path_vel - np.dot(self.K1,p - self.path_pos) + self.alpha[3:5] = self.a45 # populate error terms self.z1 = p - self.path_pos @@ -363,52 +420,60 @@ class Main: B = np.dot(C_c,self.a45) + np.dot(M_c,self.a45dot) - # Gain terms - Kp = np.identity(3) + np.dot(self.K2[:3,:3],self.K1) + self.tune*np.identity(3) - Kd = self.tot_m*self.K1 + self.K2[:3,:3] - Ki = self.tune*self.K1 - # Desired body-oriented forces # shouldnt it be tot_m*path_acc? - Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,0.5*self.dt*(self.z1 - self.z1_p)) - + Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) + # Fd = B + G[:3] + self.tot_m*self.path_acc - np.dot(self.Kd,z1_dot) - np.dot(self.Kp,self.z1) - np.dot(self.Ki,0.5*self.dt*(self.z1 - self.z1_p)) # Update self.z1_p for "integration" self.z1_p = self.z1 # Covert Fd into drone frame dr_orientation = [self.dr_pos.orientation.x, self.dr_pos.orientation.y, self.dr_pos.orientation.z, self.dr_pos.orientation.w] dr_orientation_inv = quaternion_inverse(dr_orientation) - - Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0],dr_orientation_inv)) # Real part of Fd needs = 0 - + Fd_tf = quaternion_multiply(dr_orientation,quaternion_multiply([Fd[0],Fd[1],Fd[2],0.0],dr_orientation_inv)) # Real part of Fd needs = 0 + # Fd_tf = [Fd[0],Fd[1],Fd[2]] # Convert forces to attiude *EulerAng[2] = yaw = 0 self.EulerAng[1] = math.atan(Fd_tf[0]/(self.drone_m*9.81)) # Pitch self.EulerAng[0] = math.atan(-Fd_tf[1]*math.cos(self.EulerAng[1])/(self.drone_m*9.81)) # Roll + + # rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f' % Fd[0],Fd[1],Fd[2]) + # rospy.loginfo('Fd before transform: %.2f, %.2f, %.2f', Fd[0], Fd[1], Fd[2]) + # rospy.loginfo('Fd after transform: %.2f, %.2f, %.2f', Fd_tf[0],Fd_tf[1],Fd_tf[2]) # Convert Euler angles to quaternions + # for i,val in enumerate(self.EulerAng): + # if val*-1 < 0.0: + # sign = 1 + # else: + # sign = -1 + # if abs(val) >=0.44: + # self.EulerAng[i] = 0.44*sign + q = quaternion_from_euler(self.EulerAng[0],self.EulerAng[1],self.EulerAng[2]) # Populate msg variable # Attitude control self.att_targ.header.stamp = rospy.Time.now() - self.att_targ.header.frame_id = 'map' + self.att_targ.header.frame_id = '/map' self.att_targ.type_mask = 1|2|4 self.att_targ.orientation.x = q[0] self.att_targ.orientation.y = q[1] self.att_targ.orientation.z = q[2] self.att_targ.orientation.w = q[3] - + + def user_fback(self): + rospy.loginfo('thrust: %.6f' % self.att_targ.thrust) + rospy.loginfo('\nroll: %.2f\npitch: %.2f',self.EulerAng[0]*180/3.14,self.EulerAng[1]*180/3.14) + def publisher(self,pub_time): self.determine_q() self.determine_throttle() self.pub_att_targ.publish(self.att_targ) - #rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1]) + self.user_fback() # --------------------------------------------------------------------------------# -# ALGORITHM +# ALGORITHM ENDS # --------------------------------------------------------------------------------# - - if __name__=="__main__": # Initiate ROS node diff --git a/src/offb_node.cpp b/src/offb_node.cpp index b43710b..50cecbd 100644 --- a/src/offb_node.cpp +++ b/src/offb_node.cpp @@ -48,16 +48,15 @@ void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ ROS_INFO("Waiting..."); } else { pose_read = true; - pose.pose.orientation.x = q_init.x; - pose.pose.orientation.y = q_init.y; - pose.pose.orientation.z = q_init.z; - pose.pose.orientation.w = q_init.w; + // pose.pose.orientation.x = q_init.x; + // pose.pose.orientation.y = q_init.y; + // pose.pose.orientation.z = q_init.z; + // pose.pose.orientation.w = q_init.w; } } } - int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); @@ -66,7 +65,7 @@ int main(int argc, char **argv) /********************** SUBSCRIBERS **********************/ // Get current state ros::Subscriber state_sub = nh.subscribe - ("mavros/state", 10, state_cb); + ("mavros/state", 10, state_cb); // Pose subscriber ros::Subscriber mavPose_sub = nh.subscribe @@ -91,7 +90,7 @@ int main(int argc, char **argv) // Publish attitude commands ros::Publisher att_pub = nh.advertise - ("/mavros/setpoint_attitude/attitude",10); + ("/mavros/setpoint_attitude/attitude",10); // Service Clients ros::ServiceClient arming_client = nh.serviceClient @@ -99,7 +98,7 @@ int main(int argc, char **argv) ros::ServiceClient set_mode_client = nh.serviceClient ("mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient - ("mavros/cmd/takeoff"); + ("mavros/cmd/takeoff"); //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms ros::Rate rate(20.0); @@ -163,12 +162,16 @@ int main(int argc, char **argv) last_request = ros::Time::now(); } } - // Update desired waypoints + // check if waypoints have changed desired waypoints waypoint_cl.call(wpoint_srv); pose.pose.position.x = wpoint_srv.response.xd.x; pose.pose.position.y = wpoint_srv.response.xd.y; pose.pose.position.z = wpoint_srv.response.xd.z; - + + // User info + ROS_INFO("Current Altitude: %.2f",mavPose.pose.position.z); + ROS_INFO("Desired Altitude: %.2f",pose.pose.position.z); + ROS_INFO("---------------------------"); local_pos_pub.publish(pose); diff --git a/src/path_follow.cpp b/src/path_follow.cpp index ea37884..74d536f 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -4,7 +4,7 @@ */ #include -#include +#include #include #include #include @@ -115,21 +115,6 @@ int main(int argc, char **argv) //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms ros::Rate rate_pub(25.0); - // Retrieve desired waypoints - oscillation_ctrl::WaypointTrack wpoint_srv; - wpoint_srv.request.query = false; - if (waypoint_cl.call(wpoint_srv)) - { - ROS_INFO("Waypoints received"); - } - - // populate desired waypoints - this is only for original hover as - // a change of waypoints is handled by ref_signal.py - buff_pose.pose.position.x = wpoint_srv.response.xd.x; - 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; offb_set_mode.request.custom_mode = "OFFBOARD"; @@ -139,7 +124,8 @@ int main(int argc, char **argv) arm_cmd.request.value = true; // Take off command - bool takeoff = false, att_cmds = false; + bool takeoff = false, att_cmds = false; + bool oscillation_damp = true; // Keep track of time since requests ros::Time tkoff_req = ros::Time::now(); @@ -149,10 +135,26 @@ int main(int argc, char **argv) for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(buff_pose); ros::spinOnce(); - ROS_INFO("Publishing position setpoints"); + ROS_INFO_ONCE("Publishing position setpoints"); rate_wait.sleep(); } + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)) + { + ROS_INFO("Waypoints received"); + // populate desired waypoints - this is only for original hover as + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + } else { + ROS_INFO("NO waypoints received"); + } + + double alt_des = wpoint_srv.response.xd.z; // Desired height + while(ros::ok()){ if(gps_read == true){ if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ @@ -177,26 +179,44 @@ int main(int argc, char **argv) takeoff = true; } } + + // Check if we want to use oscillation controller + //if (ros::param::get("/use_ctrl", oscillation_damp) == true){ + if (ros::param::has("/status/use_ctrl")){ + ros::param::get("/status/use_ctrl", oscillation_damp); + if(oscillation_damp == true){ + ROS_INFO("ATTITUDE CTRL"); + } + } // Determine which messages to send - if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){ + // if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff && oscillation_damp){ + // att_targ.header.stamp = ros::Time::now(); + // att_targ_pub.publish(att_targ); + // att_cmds = true; + if(oscillation_damp){ att_targ.header.stamp = ros::Time::now(); att_targ_pub.publish(att_targ); att_cmds = true; - ROS_INFO("ATTITUDE CTRL"); - ROS_INFO("Des Altitude: %.2f", alt_des); - ROS_INFO("Cur Altitude: %.2f", current_altitude); } else { + // Check if waypoints have changed + if (waypoint_cl.call(wpoint_srv)) + { + // populate desired waypoints - this is only for original hover as + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + } local_pos_pub.publish(buff_pose); ROS_INFO("POSITION CTRL"); - ROS_INFO("Des Altitude: %.2f", alt_des); - ROS_INFO("Cur Altitude: %.2f", current_altitude); - } + } + ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Cur Altitude: %.2f", current_altitude); + ROS_INFO("---------------------------"); ros::spinOnce(); rate_pub.sleep(); } } return 0; -} - +} \ No newline at end of file diff --git a/src/perform_test.py b/src/perform_test.py index 0424282..43e2b4d 100755 --- a/src/perform_test.py +++ b/src/perform_test.py @@ -27,8 +27,8 @@ class Main: # Test parameters # Square - self.xarray = [1,2,2,2,1,0,0] - self.yarray = [0,0,1,2,2,2,1] + # 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 @@ -46,6 +46,16 @@ class Main: self.test = 'none' break + if rospy.has_param('sim/square_x'): + self.xarray = rospy.get_param('sim/square_x') + self.yarray = rospy.get_param('sim/square_y') + elif rospy.has_param('mocap/square_x'): + self.xarray = rospy.get_param('mocap/square_x') + self.yarray = rospy.get_param('mocap/square_y') + else: + self.xarray = [1,2,2,2,1,0,0] + self.yarray = [0,0,1,2,2,2,1] + # get waypoints self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) @@ -77,12 +87,11 @@ class Main: if self.i >= len(self.xarray): self.Point.x = 0 self.Point.y = 0 - else: - self.Square_Point.x = self.xarray[self.i] - self.Square_Point.y = self.yarray[self.i] + self.Point.x = self.xarray[self.i] + self.Point.y = self.yarray[self.i] - self.Point = self.Square_Point + # self.Point = self.Square_Point self.j += 1 self.i = self.j // self.buffer @@ -91,7 +100,7 @@ class Main: self.Point.x = self.step_size # STEP (meters) def user_feedback(self): - rospy.loginfo("Sending [Point x] %d [Point y] %d", + rospy.loginfo("Sending [Point x] %.2f [Point y] %.2f", self.Point.x, self.Point.y) def determine_test(self): diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index e637c6e..62e7e63 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -7,30 +7,38 @@ import rospy, tf import numpy as np import time import math +import rosservice from scipy.integrate import odeint -from oscillation_ctrl.msg import RefSignal, LoadAngles +from oscillation_ctrl.msg import RefSignal, LoadAngles, TetheredStatus from oscillation_ctrl.srv import WaypointTrack from geometry_msgs.msg import Pose, PoseStamped, Point, TwistStamped from sensor_msgs.msg import Imu from mavros_msgs.msg import State +class DesiredPoint(): + def __init__(self,x,y,z): + self.x = x + self.y = y + self.z = z + class Main: def __init__(self): # rate(s) - rate = 60 # rate for the publisher method, specified in Hz -- 10 Hz + rate = 50 # rate for the publisher method, specified in Hz -- 10 Hz # initialize variables 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() - + self.dt = 1.0/rate self.tmax = self.dt # used to get desired pos, vel, and acc for next time step (tmax) self.n = self.tmax/self.dt + 1 self.t = np.linspace(0, self.tmax, self.n) # Time array + # self.t = np.linspace(0, 1.0, 10) # Time array # Message generation/ collection self.vel_data = TwistStamped() # This is needed to get drone vel from gps @@ -42,24 +50,15 @@ class Main: self.dr_pos = Pose() self.dr_vel = self.vel_data.twist.linear self.dr_acc = self.imu_data.linear_acceleration - - # Get tether length - self.param_exists = False - while self.param_exists == False: - 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 >= 10.0: - self.tetherL = 0.0 - break - + + self.tetherL = self.get_tether() # --------------------------------------------------------------------------------# -# SUBSCRIBERS # +# SUBSCRIBERS # # --------------------------------------------------------------------------------# # Topic, msg type, and class callback method rospy.Subscriber('/status/load_angles', LoadAngles, self.loadAngles_cb) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.dronePos_cb) - #rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) + # rospy.Subscriber('/status/twoBody_status', TetheredStatus, self.dronePos_cb) rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) @@ -79,11 +78,11 @@ class Main: # --------------------------------------------------------------------------------# # Smooth path variables - self.EPS_F = np.zeros(9) # Final Epsilon/ signal - self.EPS_I = np.zeros(9) # Epsilon shapefilter + self.EPS_F = np.zeros(9) # Final Epsilon/ signal + self.EPS_I = np.zeros(9) # Epsilon shapefilter # Constants for smooth path generation - self.w_tune = 3 # 3.13 works well? ######################################################################### + self.w_tune = 1 # 1 also works well :) 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio # need exception if we do not have tether: @@ -91,7 +90,7 @@ class Main: 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 @@ -99,12 +98,7 @@ class Main: self.k1 = (self.w_tune**4)/(self.k2*self.k3*self.k4) # For saturation: - self.jmax = [3, 3, 1] - self.amax = [5, 5, 1] - self.vmax = [10, 10, 1] #[3, 3, 1] - self.max = [0, 3, 1.5, 3] #[0, 3, 1.5, 3] - # create the array: [vmax; amax; jmax] - self.max_array = np.array([[self.vmax],[self.amax],[self.jmax]]).T + self.max = [0, 8, 1.5, 3] #[0, 10, 1.5, 3] self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) self.empty_point = Point() # Needed to query waypoint_server @@ -146,9 +140,11 @@ class Main: # Constants for sigmoid self.at = 3 # acceleration theshold self.pc = 0.7 # From Klausen 2017 - self.sk = len(self.SF_delay_x[0]) # from Klausen 2017 - self.ak = np.zeros(len(self.SF_delay_x[0])) + self.sk = self.SF_delay_x.shape[1] # from Klausen 2017 + self.ak = np.zeros([self.sk]) self.s_gain = 0.0 # Gain found from sigmoid + + self.service_list = rosservice.get_service_list() # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -156,7 +152,7 @@ class Main: # Callback to get link names, states, and pload deflection angles def loadAngles_cb(self,msg): try: - self.load_angles = msg + self.load_angles = msg except ValueError: pass @@ -164,7 +160,7 @@ class Main: def dronePos_cb(self,msg): try: self.dr_pos = msg.pose - #self.dr_pos = msg.drone_pos + # self.dr_pos = msg.drone_pos except ValueError: pass @@ -186,34 +182,46 @@ class Main: pass def waypoints_srv_cb(self): - rospy.wait_for_service('/status/waypoint_tracker') - try: - resp = self.get_xd(False,self.empty_point) - 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 # -###################################################################### + if '/status/waypoint_tracker' in self.service_list: + rospy.wait_for_service('/status/waypoint_tracker') + try: + resp = self.get_xd(False,self.empty_point) + self.xd = resp.xd + except ValueError: + pass + else: + self.xd = DesiredPoint(0.0,0.0,2.0) + def get_tether(self): + """ Get tether length based on set parameters""" + self.param_exists = False + while self.param_exists == False: + if rospy.has_param('status/tether_length'): + tether_length = rospy.get_param('status/tether_length') # Tether length + self.param_exists = True + elif rospy.get_time() - self.tstart >= 15: + tether_length = 0.0 + break + return tether_length # --------------------------------------------------------------------------------# # ALGORITHM # --------------------------------------------------------------------------------# # --------------------------------------------------------------------------------# # TRAJECTORY GENERATION BASED ON WAYPONTS xd # --------------------------------------------------------------------------------# + def statespace(self,y,t,xd): # Update initial conditions # # Need the statespace array: pos,vel,acc,jer = y + error = xd - pos + if abs(error) <= 0.25: error = 0.0 + # Derivative of statesape array: dydt = [vel, acc, jer, - self.k4*(self.k3*(self.k2*(self.k1*(xd - pos) - vel) - acc) - jer)] + self.k4*(self.k3*(self.k2*(self.k1*(error) - vel) - acc) - jer)] return dydt # Sigmoid @@ -244,7 +252,7 @@ class Main: else: # once array is filled, we start using the first value every time # x - self.SF_delay_x[j,:] = np.roll(self.SF_delay_x[j,:],-1) # makes the first value last + self.SF_delay_x[j,:] = np.roll(self.SF_delay_x[j,:],-1) # makes the first value last self.SF_delay_x[j,len(self.SF_delay_x[0])-1] = self.x[1,j] # updates last value to current x # y @@ -285,12 +293,13 @@ class Main: # Convolution of shape filter and trajectory, and feedback def convo(self): + # check if waypoints have changed self.waypoints_srv_cb() # needed for delay function # 1 = determine shapefilter array # 2 = determine theta/phi_fb - shapeFil = 1 - feedBack = 2 + SHAPEFIL = 1 + FEEDBACK = 2 # SOLVE ODE (get ref pos, vel, accel) self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) @@ -304,25 +313,26 @@ 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 (shapefilter) 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] - self.EPS_I[3*j+1] = self.y[1,j] + self.EPS_I[3*j] = self.x[-1,j] + self.EPS_I[3*j+1] = self.y[-1,j] #self.EPS_I[3*j+2] = self.z[1,j] else: - self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) - self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) + self.EPS_I[3*j] = self.A1*self.x[-1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) + self.EPS_I[3*j+1] = self.A1*self.y[-1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) #self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim - self.delay(1,feedBack) # Detemine feedback array + self.delay(1,FEEDBACK) # Detemine feedback array self.sigmoid() # Determine sigmoid gain EPS_D = self.fback() # Feedback Epsilon - for i in range(9): + # for i in range(9): # Populate EPS_F buffer with desired change based on feedback - self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i] + # self.EPS_F[i] = self.EPS_I[i] + self.s_gain*EPS_D[i] #+ EPS_D[i] + self.EPS_F = self.EPS_I + self.s_gain*EPS_D # Populate msg with epsilon_final self.ref_sig.header.stamp = rospy.Time.now() @@ -342,15 +352,18 @@ class Main: #self.ref_sig.velocity.z = self.EPS_F[5] #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]] - #self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] + # 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]] + # self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] + self.x0 = [self.x[1,0], self.x[1,1], self.x[1,2], self.x[1,3]] + self.y0 = [self.y[1,0], self.y[1,1], self.y[1,2], self.y[1,3]] self.SF_idx += 1 self.FB_idx += 1 def fback(self): + """ Uses first element in feedback array as this is what get replaced in each iteration """ xr = self.Gd*self.tetherL*math.sin(self.theta_fb[0]) xdotr = self.Gd*self.tetherL*math.cos(self.theta_fb[0])*self.theta_vel_fb[0] @@ -367,13 +380,14 @@ class Main: def screen_output(self): # Feedback to user - 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('_______________________') - - #rospy.loginfo('xd = %.2f',self.xd.x) + # 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('_______________________') + rospy.loginfo_once('Tether length: %.2f',self.tetherL) + # rospy.loginfo('Theta: %.2f',self.load_angles.theta) + # rospy.loginfo('Phi: %.2f',self.load_angles.phi) def publisher(self,pub_tim): @@ -382,18 +396,16 @@ class Main: # Publish reference signal self.pub_path.publish(self.ref_sig) - #self.pub_ref.publish(self.ref_sig) # for geometric controller # Give user feedback on published message: - #self.screen_output() + self.screen_output() - if __name__=="__main__": # Initiate ROS node rospy.init_node('desPath_node',anonymous=True) try: - Main() # create class object + Main() # create class object rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py index a834f27..6c7382b 100755 --- a/src/wpoint_tracker.py +++ b/src/wpoint_tracker.py @@ -31,11 +31,17 @@ class Main: self.xd.y = self.wpoints['y'] self.xd.z = self.wpoints['z'] self.param_exists = True + elif rospy.has_param('mocap/waypoints'): + self.wpoints = rospy.get_param('mocap/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: + elif rospy.get_time() - self.tstart >= 5.0: self.xd.x = 0.0 self.xd.y = 0.0 - self.xd.z = 5.0 + self.xd.z = 1.5 break # service(s)