forked from cesar.alejandro/oscillation_ctrl
Upload files to 'src'
This commit is contained in:
parent
bb148364c1
commit
4975235080
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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 <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/CommandTOL.h>
|
||||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <mavros_msgs/Thrust.h>
|
||||
#include <mavros_msgs/VFR_HUD.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
/********* 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_msgs::State>
|
||||
("mavros/state", 10, state_cb);
|
||||
|
||||
// Pose subscriber
|
||||
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("mavros/local_position/pose",10,mavPose_cb);
|
||||
|
||||
// Waypoint Subscriber
|
||||
ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("/reference/waypoints",10,waypoints_cb);
|
||||
|
||||
/********************** PUBLISHERS **********************/
|
||||
// Initiate publisher to publish commanded local position
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
|
||||
// Publish desired attitude
|
||||
ros::Publisher thrust_pub = nh.advertise<mavros_msgs::Thrust>
|
||||
("mavros/setpoint_attitude/thrust", 10);
|
||||
|
||||
// Publish attitude commands
|
||||
ros::Publisher att_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("/mavros/setpoint_attitude/attitude",10);
|
||||
|
||||
// Service Clients
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,252 @@
|
|||
/**
|
||||
* @file path_follow.cpp
|
||||
* @brief Offboard path trajectory tracking
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <offboard_ex/RefSignal.h>
|
||||
#include <offboard_ex/EulerAngles.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/CommandTOL.h>
|
||||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <mavros_msgs/Thrust.h>
|
||||
#include <mavros_msgs/VFR_HUD.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <iostream>
|
||||
#include <cmath> // 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_msgs::State>
|
||||
("mavros/state", 10, state_cb);
|
||||
|
||||
// Get quaternions from klausen control
|
||||
ros::Subscriber q_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("command/quaternions",10,q_cb);
|
||||
|
||||
// Pose subscriber
|
||||
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("mavros/local_position/pose",10,mavPose_cb);
|
||||
|
||||
// Throttle: Jaeyoung-Lim
|
||||
ros::Subscriber thro_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
|
||||
("command/bodyrate_command",10,thrust_cb);
|
||||
|
||||
/********************** PULISHERS **********************/
|
||||
// Initiate publisher to publish commanded local position
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
|
||||
// Publish attitude and thrust commands
|
||||
ros::Publisher att_targ = nh.advertise<mavros_msgs::AttitudeTarget>
|
||||
("mavros/setpoint_raw/attitude",10);
|
||||
|
||||
// Publish attitude cmds in euler angles
|
||||
ros::Publisher euler_pub = nh.advertise<offboard_ex::EulerAngles>
|
||||
("command/euler_angles",10);
|
||||
|
||||
// Service Clients
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("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;
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
Loading…
Reference in New Issue