From 7a84c2ca33a6bd044acc9720725eff6214c8ee5a Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 5 Jul 2022 11:06:45 -0300 Subject: [PATCH] Pushing latest changes and removing unecessary files --- .gitignore | 14 +- CMakeLists.txt | 2 +- launch/cortex_bridge.launch | 43 --- launch/klausen_dampen.launch | 75 ----- launch/mocap_sim.launch | 37 --- ...tethered_status.msg => TetheredStatus.msg} | 0 setup.txt | 120 -------- src/LinkState.py | 6 +- src/MoCap_Localization_Tether.py | 261 ------------------ src/MoCap_Localization_noTether.py | 183 ------------ src/klausen_control.py | 4 +- src/path_follow.cpp | 8 +- src/quarternion.py | 48 ---- src/ref_signalGen.py | 2 +- src/wpoint_tracker.py | 2 +- 15 files changed, 21 insertions(+), 784 deletions(-) delete mode 100644 launch/cortex_bridge.launch delete mode 100644 launch/klausen_dampen.launch delete mode 100644 launch/mocap_sim.launch rename msg/{tethered_status.msg => TetheredStatus.msg} (100%) delete mode 100644 setup.txt delete mode 100755 src/MoCap_Localization_Tether.py delete mode 100755 src/MoCap_Localization_noTether.py delete mode 100644 src/quarternion.py diff --git a/.gitignore b/.gitignore index 25d96a3..09c6835 100644 --- a/.gitignore +++ b/.gitignore @@ -1,12 +1,16 @@ -launch/debug.launch -launch/mocap_* launch/cortex_bridge.launch -src/MoCap_Localization_*.py -src/Mocap_*.py -src/segmented_tether.py +launch/debug.launch +launch/klausen_dampen.launch +launch/mocap_* +src/development_* src/killswitch_client.py src/land_client.py +src/MoCap_*.py +src/Mocap_*.py +src/segmented_tether.py +src/segmented_tether_fast.py msg/Marker.msg msg/Markers.msg *.rviz +setup.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 7fa04c0..476b763 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,7 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - tethered_status.msg + TetheredStatus.msg RefSignal.msg EulerAngles.msg LoadAngles.msg diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch deleted file mode 100644 index 09a685a..0000000 --- a/launch/cortex_bridge.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch deleted file mode 100644 index 3baf0ef..0000000 --- a/launch/klausen_dampen.launch +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/mocap_sim.launch b/launch/mocap_sim.launch deleted file mode 100644 index ccb42e5..0000000 --- a/launch/mocap_sim.launch +++ /dev/null @@ -1,37 +0,0 @@ - - - - - - - - - - - - - - diff --git a/msg/tethered_status.msg b/msg/TetheredStatus.msg similarity index 100% rename from msg/tethered_status.msg rename to msg/TetheredStatus.msg diff --git a/setup.txt b/setup.txt deleted file mode 100644 index cac00aa..0000000 --- a/setup.txt +++ /dev/null @@ -1,120 +0,0 @@ -Cesar Rodriguez -February 2022 - -Steps to recreate stable PX4 environment + working repo - -STEP 1) Installing ROS Melodic - -- SETUP SOURCES.LIST - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -- SETUP YOUR KEYS - sudo apt install curl # if you haven't already installed curl - curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - -- INSTALLATION - sudo apt update - sudo apt install ros-melodic-desktop-full -- ENVIRONMENT SETUP - echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc -- DEPENDENCIES - sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential - - Initilize rosdep: - sudo apt install python-rosdep - sudo rosdep init - rosdep update -- PX4 DEPENDENCIES - sudo apt-get install python-catkin-tools python-rosinstall-generator -y - wstool init ~/catkin_ws/src -- MAVLINK - rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall -- MAVROS - rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall -- CREATE WORKSPACE AND DEPS - cd ~/catkin_ws - wstool merge -t src /tmp/mavros.rosinstall - wstool update -t src -j4 - rosdep install --from-paths src --ignore-src -y -- INSTALL GEOGRAPHIC LIB DATASETS - cd ~/catkin_ws - sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh -- BUILD SOURCE - cd ~/catkin_ws - catkin build - - -STEP 2) PX4 Environment Development - -- DOWNLOAD PX4 SOURCE CODE - git clone https://github.com/PX4/PX4-Autopilot.git --recursive -- RUN UBUNTU.SH - bash ./PX4-Autopilot/Tools/setup/ubuntu.sh - Restart computer after it is done -- BUILD ROS/GAZEBO: Gets Gazebo9 - wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh - bash ubuntu_sim_ros_melodic.sh -- Download QGroundControl from: - https://docs.qgroundcontrol.com/master/en/releases/daily_builds.html -- Build jMAVSim and Gazebo - cd ~/PX4-Autopilot - make px4_sitl jmavsim - %%% May need to open QGroundControl for it to work %%% - make px4_sitl gazebo -- Create px4 package - cd ~/PX4-Autopilot - DONT_RUN=1 make px4_sitl_default gazebo - source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default - export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) - export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo - roslaunch px4 posix_sitl.launch - -STEP 3) This is Cesar stuff, need to do stuff to rebuild oscillation_ctrl -- INSTALL XTERM - sudo apt-get update -y - sudo apt-get install -y xterm --INSTALL MAVROS_CONTROLLERS - cd ~/catkin_ws/src - clone repo: - git clone https://github.com/Jaeyoung-Lim/mavros_controllers - Download dependencies: - cd ~/catkin_ws - wstool merge -t src src/mavros_controllers/dependencies.rosinstall - wstool update -t src -j4 - rosdep install --from-paths src --ignore-src -y --rosdistro $ROS_DISTRO - catkin build - source ~/catkin_ws/devel/setup.bash - ~Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers --TOOLS/SITL_GAZEBO - copy (or add) files in oscillation_ctrl/models and oscillation_ctrl/worlds to PX4-Autopilot/Tools/sitl_gazebo/models and PX4-Autopilot/Tools/sitl_gazebo/worlds respectively --ROMFS/PX4FMU_COMMON - copy (or add) files in oscillation_ctrl/airframes to PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes - add file name to CmakeLists.txt in same 'airframe' folder (with number) - add airframe name in ~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake (no number!) --LAUNCH FILES - ccopy (or add) files from px4_launch directory to ~/PX4-Autopilot/launch --MAVROS - - in px4.launch, replace: - - - - with: - - -- CHANGE DEVEL/SETUP.BASH - In catkin_ws (or any working directory) add to devel/setup.bash: - CURRENT_DIR=$(pwd) - cd ~/PX4-Autopilot - - source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default - export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd) - export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo - - cd $CURRENT_DIR - -JINJA TETHER FILE -- 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) - - - - - diff --git a/src/LinkState.py b/src/LinkState.py index bc97ef5..5ea046e 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -8,7 +8,7 @@ import rosservice import time import math from tf.transformations import * -from oscillation_ctrl.msg import tethered_status, LoadAngles +from oscillation_ctrl.msg import TetheredStatus, LoadAngles from geometry_msgs.msg import Pose from gazebo_msgs.srv import GetLinkState from std_msgs.msg import Bool @@ -41,7 +41,7 @@ class Main: self.bool = False # variables for message gen - self.status = tethered_status() + self.status = TetheredStatus() self.drone_id = 'spiri_with_tether::spiri::base' self.pload_id = 'spiri_with_tether::mass::payload' self.loadAngles = LoadAngles() @@ -64,7 +64,7 @@ class Main: rospy.wait_for_service(self.service1,timeout=10) # publisher(s) - self.twobody_pub = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) + self.twobody_pub = rospy.Publisher('/status/twoBody_status', TetheredStatus, queue_size=1) self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1) self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1) diff --git a/src/MoCap_Localization_Tether.py b/src/MoCap_Localization_Tether.py deleted file mode 100755 index 254ac67..0000000 --- a/src/MoCap_Localization_Tether.py +++ /dev/null @@ -1,261 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Feb 2022 -### Script to determine payload and drone state using mocap - -import rospy, tf -import rosservice -import time -import math -from tf.transformations import * -from offboard_ex.msg import tethered_status -from geometry_msgs.msg import Pose -from gazebo_msgs.srv import GetLinkState -from std_msgs.msg import Bool - -class Main: - - def __init__(self): - - # 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 - self.tstart = rospy.get_time() - - ### -*-*-*- Do not need this unless a test is being ran -*-*-*- ### - # How long should we wait before before starting test - #self.param_exists = False - #while self.param_exists == False: - # if rospy.has_param('sim/wait'): - # self.wait = rospy.get_param('sim/wait') # wait time - # self.param_exists = True - # elif rospy.get_time() - self.tstart >= 3.0: - # break - - # Will be set to true when test should start - #self.bool = False - ### -*-*-*- END -*-*-*- ### - - # initialize variables - self.phi = 0.0 # Payload angle of deflection from x-axis - self.theta = 0.0 # Payload angle of deflection from y-axis - self.tetherL = 0.0 # Tether length - self.has_run = 0 # Boolean to keep track of first run instance - self.phidot = 0.0 # - self.thetadot = 0.0 # - self.phibuf = 0.0 # Need buffers to determine their rates - self.thetabuf = 0.0 # - self.pload = True # Check if payload exists - # Max dot values to prevent 'blowup' - self.phidot_max = 3.0 - self.thetadot_max = 3.0 - - # variables for message gen - self.status = tethered_status() - self.status.drone_id = 'spiri_with_tether::spiri::base' - self.status.drone_pos = Pose() - self.status.pload_id = 'spiri_with_tether::mass::payload' - self.status.pload_pos = Pose() - self.status.phi = 0.0 - self.status.theta = 0.0 - - # service(s) - self.service1 = '/gazebo/get_link_state' - - # need service list to check if models have spawned - self.service_list = rosservice.get_service_list() - - # wait for service to exist - while self.service1 not in self.service_list: - print "Waiting for models to spawn..." - self.service_list = rosservice.get_service_list() - if rospy.get_time() - self.tstart >= 10.0: - break - - # 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) - - def cutoff(self,value,ceiling): - """ - Takes in value and returns ceiling - if value > ceiling. Otherwise, it returns - value back - """ - # initilize sign - sign = 1 - - # check if value is negative - if value < 0.0: - sign = -1 - # Cutoff value at ceiling - if (value > ceiling or value < -ceiling): - output = sign*ceiling - else: - output = value - return output - - def euler_array(self,pose): - """ - Takes in pose msg object and outputs array of euler angs: - q[0] = Roll - q[1] = Pitch - q[2] = Yaw - """ - q = euler_from_quaternion([pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w]) - return q - -# def FRD_Transform(pose) -# ''' -# Transforms mocap reading to proper coordinate frame -# ''' -# pose.position.x = -# pose.position.y = -# pose.position.z = - -# Keep the w same and change x, y, and z as above. -# pose.orientation.x = -# pose.orientation.y = -# pose.orientation.z = -# pose.orientation.w = - -# return pose - - # Get link states (drone and pload) and determine angle between them - def link_state(self,pub_timer): - - try: - - # State which service we are querying - get_P = rospy.ServiceProxy(self.service1,GetLinkState) - - # Set reference frame - reference = '' - - # Establish links needed --> Spiri base and payload - # P = Position vector - drone_P = get_P(self.status.drone_id,reference) - - # Get orientation of drone in euler angles - drone_Eul = self.euler_array(drone_P.link_state.pose) - - # Check if payload is part of simulation - if not drone_P.success: - self.status.drone_id = 'spiri::base' - drone_P = get_P(self.status.drone_id,reference) # i.e. no payload - self.pload = False - - pload_P = get_P(self.status.pload_id,reference) - - if not self.has_run == 1: - if self.pload == True: - # 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 + - (drone_P.link_state.pose.position.y - - 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) - - else: - self.tetherL = 0 - - self.has_run = 1 - - # Need to detemine their location to get angle of deflection - # Drone - drone_Px = drone_P.link_state.pose.position.x - drone_Py = drone_P.link_state.pose.position.y - drone_Pz = drone_P.link_state.pose.position.z - - if self.pload == True: # If there is payload, determine the variables - # Pload - pload_Px = pload_P.link_state.pose.position.x - pload_Py = pload_P.link_state.pose.position.y - - # Determine theta (pitch) - x_sep = pload_Px - drone_Px - - if math.fabs(x_sep) >= self.tetherL or x_sep == 0: - self.theta = 0 - else: - self.theta = math.asin(x_sep/self.tetherL) - - # Determine thetadot - self.thetadot = (self.theta - self.thetabuf)/self.dt - self.thetadot = self.cutoff(self.thetadot,self.thetadot_max) - self.thetabuf = self.theta - - # Determine phi (roll) - y_sep = pload_Py - drone_Py - - if math.fabs(y_sep) >= self.tetherL or y_sep == 0: - self.phi = 0 - else: - self.phi = math.asin(y_sep/self.tetherL) - - # Determine phidot - self.phidot = (self.phi - self.phibuf)/self.dt - self.phidot = self.cutoff(self.phidot,self.phidot_max) - self.phibuf = self.phi # Update buffer - - else: # Otherwise, vars = 0 - x_sep = self.phi = self.phidot = self.theta = self.thetadot = 0 - - # 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 "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)) - print "phi: " + str(round(self.phi*180/3.14,3)) - print "theta: " + str(round(self.theta*180/3.14,3)) - - # Populate message - self.status.drone_pos = drone_P.link_state.pose - self.status.pload_pos = pload_P.link_state.pose - self.status.length = self.tetherL - self.status.phi = self.phi - self.status.phidot = self.phidot - self.status.theta = self.theta - self.status.thetadot = self.thetadot - - # Publish message - self.publisher.publish(self.status) - - except rospy.ServiceException as e: - rospy.loginfo("Get Link State call failed: {0}".format(e)) - -# def path_follow(self,path_timer): -# now = rospy.get_time() -# if now - self.tstart < self.wait: -# self.bool = False -# else: -# self.bool = True -# self.pub_wd.publish(self.bool) - - -if __name__=="__main__": - - # Initiate ROS node - rospy.init_node('linkStates_node',anonymous=False) - try: - Main() # create class object - rospy.spin() # loop until shutdown signal - - except rospy.ROSInterruptException: - pass - diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py deleted file mode 100755 index 1fcd148..0000000 --- a/src/MoCap_Localization_noTether.py +++ /dev/null @@ -1,183 +0,0 @@ -#!/usr/bin/env python2.7 - -### Cesar Rodriguez Feb 2022 -### Script to determine payload and drone state using mocap - -import rospy, tf -import rosservice -import time -import math -from tf.transformations import * -from oscillation_ctrl.msg import tethered_status -from geometry_msgs.msg import PoseStamped, Point -from std_msgs.msg import Bool - -class Main: - - def __init__(self): - - # rate(s) - rate = 120 # rate for the publisher method, specified in Hz -- 20 Hz - - # 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 - self.tstart = rospy.get_time() - - ### -*-*-*- Do not need this unless a test is being ran -*-*-*- ### - # How long should we wait before before starting test - #self.param_exists = False - #while self.param_exists == False: - # if rospy.has_param('sim/wait'): - # self.wait = rospy.get_param('sim/wait') # wait time - # self.param_exists = True - # elif rospy.get_time() - self.tstart >= 3.0: - # break - - # Will be set to true when test should start - #self.bool = False - ### -*-*-*- END -*-*-*- ### - - # initialize variables - self.drone_pose = PoseStamped() - self.buff_pose = PoseStamped() - - self.eul = [0.0,0.0,0.0] - - # Max dot values to prevent 'blowup' - self.phidot_max = 3.0 - self.thetadot_max = 3.0 - - # variables for message gen - - - # service(s) - - # need service list to check if models have spawned - - - # wait for service to exist - - - # publisher(s) - ### Since there is no tether, we can publish directly to mavros - self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) - - self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) - - # subscriber(s) - rospy.Subscriber('/cortex/body_pose', PoseStamped, self.bodyPose_cb) - - def cutoff(self,value,ceiling): - """ - Takes in value and returns ceiling - if value > ceiling. Otherwise, it returns - value back - """ - # initilize sign - sign = 1 - - # check if value is negative - if value < 0.0: - sign = -1 - # Cutoff value at ceiling - if (value > ceiling or value < -ceiling): - output = sign*ceiling - else: - output = value - return output - - def euler_array(self): - """ - Takes in pose msg object and outputs array of euler angs: - eul[0] = Roll - eul[1] = Pitch - eul[2] = Yaw - """ - self.eul = euler_from_quaternion([self.drone_pose.pose.orientation.x, - self.drone_pose.pose.orientation.y, - self.drone_pose.pose.orientation.z, - self.drone_pose.pose.orientation.w]) - - self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2]) - - offset_yaw = math.pi/2 - q_offset = quaternion_from_euler(0,0,-offset_yaw) - - self.q = quaternion_multiply(self.q,q_offset) - - self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]]) - self.drone_pose.pose.orientation.x = self.q[0] - self.drone_pose.pose.orientation.y = self.q[1] - self.drone_pose.pose.orientation.z = self.q[2] - self.drone_pose.pose.orientation.w = self.q[3] - - def FRD_Transform(self): - ''' - Transforms mocap reading to proper coordinate frame - ''' - -# self.drone_pose = self.buff_pose - 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 - -# Keep the w same and change x, y, and z as above. -# self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y -# self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x -# self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z -# self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w - -# def path_follow(self,path_timer): -# now = rospy.get_time() -# if now - self.tstart < self.wait: -# self.bool = False -# else: -# self.bool = True -# self.pub_wd.publish(self.bool) - - def bodyPose_cb(self,msg): - try: - self.drone_pose = msg - - except ValueError: - pass - - def publisher(self,pub_timer): - self.FRD_Transform() - self.pose_pub.publish(self.drone_pose) - print "\n" - rospy.loginfo("") - 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__": - - # Initiate ROS node - rospy.init_node('MoCap_node',anonymous=False) - try: - Main() # create class object - rospy.spin() # loop until shutdown signal - - except rospy.ROSInterruptException: - pass - diff --git a/src/klausen_control.py b/src/klausen_control.py index 3b905d4..5f4b44a 100755 --- a/src/klausen_control.py +++ b/src/klausen_control.py @@ -25,7 +25,7 @@ class Main: def __init__(self): # rate(s) - rate = 200 # rate for the publisher method, specified in Hz -- 20 Hz + rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz # initialize variables @@ -116,7 +116,7 @@ class Main: # 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', tethered_status, 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) diff --git a/src/path_follow.cpp b/src/path_follow.cpp index cd11285..ea37884 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -96,13 +96,13 @@ int main(int argc, char **argv) ("status/waypoint_tracker"); //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms - ros::Rate rate(20.0); + ros::Rate rate_wait(20.0); // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); ROS_INFO("Waiting for FCU connection"); - rate.sleep(); + rate_wait.sleep(); } if (current_state.connected){ @@ -113,7 +113,7 @@ int main(int argc, char **argv) /*********** Initiate variables ************************/ //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms - ros::Rate rate_pub(75.0); + ros::Rate rate_pub(25.0); // Retrieve desired waypoints oscillation_ctrl::WaypointTrack wpoint_srv; @@ -150,7 +150,7 @@ int main(int argc, char **argv) local_pos_pub.publish(buff_pose); ros::spinOnce(); ROS_INFO("Publishing position setpoints"); - rate.sleep(); + rate_wait.sleep(); } while(ros::ok()){ diff --git a/src/quarternion.py b/src/quarternion.py deleted file mode 100644 index f10c40e..0000000 --- a/src/quarternion.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -import rospy, tf -import numpy as np -import time -import math - -from tf.transformations import * -from geometry_msgs.msg import PoseStamped, Quaternion - -class Main: - def __init__(self): - - # rate - rate = 40 - self.Euler = [0,0,1.571] - self.pose = PoseStamped() - self.q = [0,0,0,0] - - self.pub_q = rospy.Publisher('/quaternions',PoseStamped,queue_size = 10) - self.pub_time = rospy.Timer(rospy.Duration(1.0/rate),self.publisher) - - def publisher(self,pub_tim): - q = quaternion_from_euler(self.Euler[0],self.Euler[1],self.Euler[2]) - #q_msg = Quaternion(q[0],q[1],q[2],q[3]) - - self.pose.header.stamp = rospy.Time.now() - self.pose.pose.position.x = 0 - self.pose.pose.position.y = 0 - self.pose.pose.position.z = 2.5 - self.pose.pose.orientation.x = q[0] - self.pose.pose.orientation.y = q[1] - self.pose.pose.orientation.z = q[2] - self.pose.pose.orientation.w = q[3] - - self.pub_q.publish(self.pose) - - rospy.loginfo("Fly to altitude: %.2f m",self.pose.pose.position.z) - -if __name__=="__main__": - rospy.init_node('quart_node') - try: - Main() - rospy.spin() - - except rospy.ROSInterruptException: - pass - diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index bfdfc6a..e637c6e 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -59,7 +59,7 @@ class Main: # 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', tethered_status, 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) diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py index 150c934..a834f27 100755 --- a/src/wpoint_tracker.py +++ b/src/wpoint_tracker.py @@ -6,7 +6,7 @@ import rospy, tf import rosservice import time -from geometry_msgs.msg import Point,Pose +from geometry_msgs.msg import Point from oscillation_ctrl.srv import WaypointTrack,WaypointTrackResponse class Main: