added straightline guidance control
This commit is contained in:
parent
5836e59301
commit
f93f526f37
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="model" default="headless_spiri_with_tether"/>
|
||||
<arg name='test' default="none"/>
|
||||
<arg name='turtle' default="none"/>
|
||||
<group ns="sim">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/noCtrl_param.yaml" />
|
||||
</group>
|
||||
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="LinkState.py"
|
||||
name="LinkStates"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="straight_line.py"
|
||||
name="circle_acceleration"
|
||||
|
||||
/>
|
||||
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
||||
<group if="$(eval test != 'none')">
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="perform_test.py"
|
||||
name="test_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</group>
|
||||
<group if="$(eval turtle != 'none')">
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="turtle_test.py"
|
||||
name="turtle_test"
|
||||
/>
|
||||
<node
|
||||
pkg="turtlesim"
|
||||
type="turtlesim_node"
|
||||
name="turtlesim_node"
|
||||
/>
|
||||
</group>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,158 @@
|
|||
#!/usr/bin/env python2.7
|
||||
|
||||
|
||||
import numpy as np
|
||||
import rospy
|
||||
from geometry_msgs.msg import Point
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from geometry_msgs.msg import Vector3Stamped
|
||||
from mavros_msgs.msg import State
|
||||
from mavros_msgs.srv import SetMode, SetModeRequest, CommandBool
|
||||
#callback to update curent pose
|
||||
def pose_callback(data):
|
||||
global pose
|
||||
pose = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z])
|
||||
|
||||
def velocity_callback(data):
|
||||
global velocity
|
||||
velocity = np.array([data.twist.linear.x,data.twist.linear.y,data.twist.linear.z])
|
||||
|
||||
def state_callback(data):
|
||||
global state
|
||||
state=data
|
||||
|
||||
|
||||
|
||||
G=9.8
|
||||
def convert_to_cylindrical(point):
|
||||
recentered = point-start_point
|
||||
y=np.dot(recentered, unit_vector)
|
||||
circle_portion = recentered-y*unit_vector
|
||||
r=np.linalg.norm(circle_portion)
|
||||
horizontal_position=np.dot(circle_portion, unit_vector_2)
|
||||
theta=np.arccos(np.linalg.norm(horizontal_position)/r)
|
||||
if(np.dot(circle_portion,unit_vector_3)<0):
|
||||
theta=2*np.pi-theta
|
||||
return y, r, theta, circle_portion
|
||||
|
||||
def get_error_acceleration(circle_position, r, velocity):
|
||||
velocity_unit_2 = np.dot(velocity, unit_vector_2)
|
||||
velocity_unit_3 = np.dot(velocity, unit_vector_3)
|
||||
circle_unit_2 = np.dot(circle_position, unit_vector_2)
|
||||
circle_unit_3 = np.dot(circle_position, unit_vector_3)
|
||||
accel = -1*(b3*velocity_unit_2+k*circle_unit_2)*unit_vector_2
|
||||
accel = accel - (b*velocity_unit_3+k*circle_unit_3)*unit_vector_3
|
||||
#velocity_in_r = np.dot(circle_position, velocity)/r
|
||||
|
||||
#accel = -1*(b*velocity_in_r*circle_position/r + k*circle_position)/m
|
||||
return accel
|
||||
|
||||
def get_forward_acceleration(y, velocity):
|
||||
forward_velocity=np.dot(velocity,unit_vector)
|
||||
forward_acceleration = 2*(9.5-forward_velocity)
|
||||
forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m
|
||||
return min(forward_acceleration, forward_acceleration_2)*unit_vector
|
||||
|
||||
return forward_acceleration
|
||||
|
||||
|
||||
def accel_publisher():
|
||||
|
||||
pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10)
|
||||
|
||||
setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
|
||||
rospy.wait_for_service('mavros/set_mode')
|
||||
rospy.wait_for_service('mavros/cmd/arming')
|
||||
arming_srv = rospy.ServiceProxy('mavros/cmd/arming',CommandBool)
|
||||
set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
|
||||
|
||||
|
||||
rate = rospy.Rate(20)
|
||||
while not rospy.is_shutdown() and not state.connected:
|
||||
|
||||
rate.sleep()
|
||||
rospy.loginfo('connected')
|
||||
initial_pose=PoseStamped()
|
||||
initial_pose.pose.position.x = 0
|
||||
initial_pose.pose.position.y = 0
|
||||
initial_pose.pose.position.z = 12
|
||||
for i in range(100):
|
||||
setpointpub.publish(initial_pose)
|
||||
rate.sleep()
|
||||
last_request = rospy.get_rostime().secs
|
||||
offb_set_mode = SetModeRequest()
|
||||
offb_set_mode.custom_mode="OFFBOARD"
|
||||
arm = True
|
||||
|
||||
while not rospy.is_shutdown() and (pose[2]<9.5 or velocity[0]>1 or velocity[1]>1):
|
||||
if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)):
|
||||
rospy.loginfo('setting mode')
|
||||
res=set_mode_srv(offb_set_mode)
|
||||
|
||||
if(not res.mode_sent):
|
||||
rospy.loginfo('Mode not sent')
|
||||
else:
|
||||
rospy.loginfo('Mode sent')
|
||||
last_request=rospy.get_rostime().secs
|
||||
else:
|
||||
if (not state.armed and (rospy.get_rostime().secs-last_request>3)):
|
||||
rospy.loginfo('arming')
|
||||
arming_srv(arm)
|
||||
last_request=rospy.get_rostime().secs
|
||||
initial_pose.pose.position.x =pose[0]
|
||||
initial_pose.pose.position.y=pose[1]
|
||||
setpointpub.publish(initial_pose)
|
||||
global start_point
|
||||
start_point=pose
|
||||
rospy.logwarn('x: %f y: %f z: %f' %(pose[0], pose[1], pose[2]))
|
||||
while not rospy.is_shutdown():
|
||||
y,r,theta,circle_position=convert_to_cylindrical(pose)
|
||||
forward_accel = get_forward_acceleration(y,velocity)
|
||||
error_accel = get_error_acceleration(circle_position,r,velocity)
|
||||
#accel = get_error_acceleration(circle_position, r, velocity) + get_forward_acceleration(y,velocity)
|
||||
#accel = get_forward_acceleration(y,velocity)
|
||||
#if(np.linalg.norm(velocity)>8) and (np.linalg.norm(forward_accel)>0):
|
||||
# forward_accel = -0*unit_vector
|
||||
accel = forward_accel+error_accel
|
||||
magnitude_accel=np.linalg.norm(accel)
|
||||
if magnitude_accel>2*G:
|
||||
accel=accel/magnitude_accel*2*G
|
||||
#if np.linalg.norm(velocity)>8:
|
||||
# accel = [0,0,0]
|
||||
#accel+=get_error_acceleration(circle_position,r,velocity)
|
||||
|
||||
to_publish=Vector3Stamped()
|
||||
to_publish.vector.x = accel[0]
|
||||
to_publish.vector.y=accel[1]
|
||||
to_publish.vector.z=accel[2]+2
|
||||
pub.publish(to_publish)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
pose = None
|
||||
velocity = None
|
||||
k = 2
|
||||
|
||||
k2 = 0.5
|
||||
|
||||
m=1.5
|
||||
b3 = 1*np.sqrt(m*k)
|
||||
b = 0.4*np.sqrt(m*k)
|
||||
b2 = 2*np.sqrt(m*k2)
|
||||
unit_vector = np.array([1,0,0])
|
||||
unit_vector_2 = np.array([0,1,0])
|
||||
unit_vector_3 = np.array([0,0,1])
|
||||
unit_vector = unit_vector/np.linalg.norm(unit_vector)
|
||||
unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2)
|
||||
|
||||
state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0)
|
||||
rospy.init_node('circle_accelerations',anonymous=True)
|
||||
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)
|
||||
rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
|
||||
rospy.Subscriber('mavros/state', State, state_callback)
|
||||
try:
|
||||
accel_publisher()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
Loading…
Reference in New Issue