From 497523508028f5fbbecd76fc5bdba75657e56ce2 Mon Sep 17 00:00:00 2001 From: "cesar.alejandro" Date: Tue, 1 Mar 2022 18:09:13 +0000 Subject: [PATCH] Upload files to 'src' --- src/LinkState.py | 242 +++++++++++++++++++++++++++++++++++ src/MoCap_Localization.py | 261 ++++++++++++++++++++++++++++++++++++++ src/offb_node.cpp | 172 +++++++++++++++++++++++++ src/path_follow.cpp | 252 ++++++++++++++++++++++++++++++++++++ src/quarternion.py | 48 +++++++ 5 files changed, 975 insertions(+) create mode 100644 src/LinkState.py create mode 100644 src/MoCap_Localization.py create mode 100644 src/offb_node.cpp create mode 100644 src/path_follow.cpp create mode 100644 src/quarternion.py diff --git a/src/LinkState.py b/src/LinkState.py new file mode 100644 index 0000000..3012bd9 --- /dev/null +++ b/src/LinkState.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python2.7 + +### Cesar Rodriguez June 2021 +### Script to determine payload and drone state from Gazebo + +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() + # 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 + + # 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 + + # 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.py b/src/MoCap_Localization.py new file mode 100644 index 0000000..254ac67 --- /dev/null +++ b/src/MoCap_Localization.py @@ -0,0 +1,261 @@ +#!/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/offb_node.cpp b/src/offb_node.cpp new file mode 100644 index 0000000..9e2f61c --- /dev/null +++ b/src/offb_node.cpp @@ -0,0 +1,172 @@ +/** + * @file offb_node.cpp + * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight + * Stack and tested in Gazebo SITL + * Cesar: Now used for path following on topic \path + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/********* CALLBACK FUNCTIONS **********************/ +// Initiate variables +mavros_msgs::State current_state; +geometry_msgs::PoseStamped desPose; + +// Callback function which will save the current state of the autopilot. +// Allows to check connection, arming, and Offboard tags*/ +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +// Cb to recieve pose information +// Initiate variables +geometry_msgs::PoseStamped pose; +geometry_msgs::Quaternion q_init; +geometry_msgs::PoseStamped mavPose; +bool pose_read = false; +double current_altitude; + +void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + mavPose = *msg; + current_altitude = mavPose.pose.position.z; + while(pose_read == false){ + q_init = mavPose.pose.orientation; + if(q_init.x == 0.0 && q_init.w == 0.0){ + 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; + } + } + +} + +// Determine new waypoins +geometry_msgs::PoseStamped wpoints; +bool des_waypoints = true; +void waypoints_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + wpoints = *msg; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "offb_node"); + ros::NodeHandle nh; + + /********************** SUBSCRIBERS **********************/ + // Get current state + ros::Subscriber state_sub = nh.subscribe + ("mavros/state", 10, state_cb); + + // Pose subscriber + ros::Subscriber mavPose_sub = nh.subscribe + ("mavros/local_position/pose",10,mavPose_cb); + + // Waypoint Subscriber + ros::Subscriber waypoint_sub = nh.subscribe + ("/reference/waypoints",10,waypoints_cb); + + /********************** PUBLISHERS **********************/ + // Initiate publisher to publish commanded local position + ros::Publisher local_pos_pub = nh.advertise + ("mavros/setpoint_position/local", 10); + + // Publish desired attitude + ros::Publisher thrust_pub = nh.advertise + ("mavros/setpoint_attitude/thrust", 10); + + // Publish attitude commands + ros::Publisher att_pub = nh.advertise + ("/mavros/setpoint_attitude/attitude",10); + + // Service Clients + ros::ServiceClient arming_client = nh.serviceClient + ("mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + ("mavros/set_mode"); + ros::ServiceClient takeoff_cl = nh.serviceClient + ("mavros/cmd/takeoff"); + + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate(20.0); + + // wait for FCU connection + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + rate.sleep(); + } + + // Populate pose msg + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 5.0; + 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; + + + //send a few setpoints before starting + for(int i = 100; ros::ok() && i > 0; --i){ + local_pos_pub.publish(pose); + ros::spinOnce(); + rate.sleep(); + } + + mavros_msgs::SetMode offb_set_mode; + offb_set_mode.request.custom_mode = "OFFBOARD"; + + mavros_msgs::CommandBool arm_cmd; + arm_cmd.request.value = true; + + ros::Time last_request = ros::Time::now(); + + while(ros::ok()){ + if(current_state.mode != "OFFBOARD" && + (ros::Time::now() - last_request > ros::Duration(5.0))){ + if( set_mode_client.call(offb_set_mode) && + offb_set_mode.response.mode_sent){ + ROS_INFO("Offboard enabled"); + } + last_request = ros::Time::now(); + } else { + if( !current_state.armed && + (ros::Time::now() - last_request > ros::Duration(3.0))){ + if(arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } + } + if(des_waypoints == true){ + pose.pose.position.x = wpoints.pose.position.x; + pose.pose.position.y = wpoints.pose.position.y; + } + + local_pos_pub.publish(pose); + + ros::spinOnce(); + rate.sleep(); + } + + return 0; +} + + diff --git a/src/path_follow.cpp b/src/path_follow.cpp new file mode 100644 index 0000000..917f386 --- /dev/null +++ b/src/path_follow.cpp @@ -0,0 +1,252 @@ +/** + * @file path_follow.cpp + * @brief Offboard path trajectory tracking +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // for std::abs + +/********* CALLBACK FUNCTIONS **********************/ +// Initiate variables +mavros_msgs::State current_state; + +geometry_msgs::PoseStamped desPose; + +tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent; +double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd +geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg +offboard_ex::EulerAngles eulAng; + +mavros_msgs::AttitudeTarget thrust; + +// Callback function which will save the current state of the autopilot. +// Allows to check connection, arming, and Offboard tags*/ +void state_cb(const mavros_msgs::State::ConstPtr& msg){ + current_state = *msg; +} + +geometry_msgs::Quaternion q_des; +double alt_des; // Check desired height + +// Cb to determine desired pose *Only needed for orientation +void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + desPose = *msg; + alt_des = desPose.pose.position.z; + q_des = desPose.pose.orientation; + tf2::convert(q_des,q_cmd); // Convert msg type to tf2::quaternion type +} + + +// Cb to recieve pose information +geometry_msgs::PoseStamped buff_pose; +geometry_msgs::Quaternion q_init; +geometry_msgs::PoseStamped mavPose; +bool gps_read = false; +double current_altitude; + +void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ + mavPose = *msg; + current_altitude = mavPose.pose.position.z; + while(gps_read == false){ + q_init = mavPose.pose.orientation; + if(q_init.x == 0.0 && q_init.w == 0.0){ + ROS_INFO("Waiting..."); + } else { + gps_read = true; + buff_pose.pose.orientation.x = q_init.x; + buff_pose.pose.orientation.y = q_init.y; + buff_pose.pose.orientation.z = q_init.z; + buff_pose.pose.orientation.w = q_init.w; + } + } +} + +// To determine throttle ... type +void thrust_cb(const mavros_msgs::AttitudeTarget msg){ + thrust = msg; + tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "path_follow"); + ros::NodeHandle nh; + + /********************** SUBSCRIBERS **********************/ + // Get current state + ros::Subscriber state_sub = nh.subscribe + ("mavros/state", 10, state_cb); + + // Get quaternions from klausen control + ros::Subscriber q_sub = nh.subscribe + ("command/quaternions",10,q_cb); + + // Pose subscriber + ros::Subscriber mavPose_sub = nh.subscribe + ("mavros/local_position/pose",10,mavPose_cb); + + // Throttle: Jaeyoung-Lim + ros::Subscriber thro_sub = nh.subscribe + ("command/bodyrate_command",10,thrust_cb); + + /********************** PULISHERS **********************/ + // Initiate publisher to publish commanded local position + ros::Publisher local_pos_pub = nh.advertise + ("mavros/setpoint_position/local", 10); + + // Publish attitude and thrust commands + ros::Publisher att_targ = nh.advertise + ("mavros/setpoint_raw/attitude",10); + + // Publish attitude cmds in euler angles + ros::Publisher euler_pub = nh.advertise + ("command/euler_angles",10); + + // Service Clients + ros::ServiceClient arming_client = nh.serviceClient + ("mavros/cmd/arming"); + ros::ServiceClient set_mode_client = nh.serviceClient + ("mavros/set_mode"); + ros::ServiceClient takeoff_cl = nh.serviceClient + ("mavros/cmd/takeoff"); + + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate(20.0); + + // wait for FCU connection + while(ros::ok() && !current_state.connected){ + ros::spinOnce(); + ROS_INFO("Waiting for FCU connection"); + rate.sleep(); + } + + /*********** Initiate variables ************************/ + // Needed for attitude command + mavros_msgs::AttitudeTarget attitude; + + attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO + attitude.type_mask = 1|2|4; // Ignore body rates + + // Need to send pose commands until throttle has been established + buff_pose.pose.position.x = 0.0; + buff_pose.pose.position.y = 0.0; + buff_pose.pose.position.z = 4.5; + + // Desired mode is offboard + mavros_msgs::SetMode offb_set_mode; + offb_set_mode.request.custom_mode = "OFFBOARD"; + + // Arm UAV + mavros_msgs::CommandBool arm_cmd; + arm_cmd.request.value = true; + + // Take off command + bool takeoff = false, att_cmds = false; + + // Keep track of time since requests + ros::Time tkoff_req = ros::Time::now(); + ros::Time last_request = ros::Time::now(); + + //send a few setpoints before starting + for(int i = 100; ros::ok() && i > 0; --i){ + local_pos_pub.publish(buff_pose); + ros::spinOnce(); + rate.sleep(); + } + + while(ros::ok()){ + if(gps_read == true){ + // Now need to convert publish q to euler ang + tf2::Matrix3x3 m_K(q_cmd); tf2::Matrix3x3 m_J(q_Jae); + m_K.getRPY(rol_K, pch_K, yaw_K); + m_J.getRPY(rol_J, pch_J, yaw_J); + + // Populate msg + K_ang.x = 180*rol_K/3.141; + K_ang.y = 180*pch_K/3.141; + K_ang.z = yaw_K; + + J_ang.x = rol_J; + J_ang.y = pch_J; + J_ang.z = yaw_J; + + eulAng.commanded = K_ang; + eulAng.desired = J_ang; + + if(current_state.mode != "OFFBOARD" && + (ros::Time::now() - last_request > ros::Duration(5.0))){ + if( set_mode_client.call(offb_set_mode) && + offb_set_mode.response.mode_sent){ + ROS_INFO("Offboard enabled"); + } else { + ROS_INFO("Could not enter offboard mode"); + } + last_request = ros::Time::now(); + } else { + if( !current_state.armed && + (ros::Time::now() - last_request > ros::Duration(3.0))){ + if( arming_client.call(arm_cmd) && + arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); + } + last_request = ros::Time::now(); + } + } + + if(current_state.mode == "OFFBOARD" && current_state.armed){ + ROS_INFO_ONCE("Spiri is taking off"); + if(!takeoff){ + tkoff_req = ros::Time::now(); + takeoff = true; + } + } + attitude.thrust = thrust.thrust; + + // Determine which messages to send + if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){ + attitude.orientation = q_des; + attitude.header.stamp = ros::Time::now(); + att_targ.publish(attitude); + att_cmds = true; + } else { + local_pos_pub.publish(buff_pose); + } + + if(!att_cmds && ros::Time::now() - last_request > ros::Duration(2.0)){ + ROS_INFO("Thro: %.2f", attitude.thrust); + } else { + ROS_INFO("Thro: %.2f", attitude.thrust); + ROS_INFO("Des Altitude: %.2f", alt_des); + ROS_INFO("Cur Altitude: %.2f", current_altitude); + //ROS_INFO("Sending points:\nRoll = %.2f\nPitch = %.2f", + // 180*rol_K/3.141,180*pch_K/3.141); + } + + // Publish euler angs + euler_pub.publish(eulAng); + + ros::spinOnce(); + rate.sleep(); + } + } + + return 0; +} + diff --git a/src/quarternion.py b/src/quarternion.py new file mode 100644 index 0000000..f10c40e --- /dev/null +++ b/src/quarternion.py @@ -0,0 +1,48 @@ +#!/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 +