diff --git a/launch/spiri_straight_line.launch b/launch/spiri_straight_line.launch new file mode 100644 index 0000000..1bc9925 --- /dev/null +++ b/launch/spiri_straight_line.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/straight_line.py b/src/straight_line.py new file mode 100755 index 0000000..d04bd67 --- /dev/null +++ b/src/straight_line.py @@ -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 +