diff --git a/launch/spiri_circle_acceleration.launch b/launch/spiri_circle_acceleration.launch new file mode 100644 index 0000000..f1a8b68 --- /dev/null +++ b/launch/spiri_circle_acceleration.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/circle_acceleration.py b/src/circle_acceleration.py new file mode 100755 index 0000000..3c92ab7 --- /dev/null +++ b/src/circle_acceleration.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python +#license removed for brevity +import rospy +import numpy as np +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 = data.pose.position + +def velocity_callback(data): + global velocity + velocity = data.twist.linear + +def state_callback(data): + global state + state=data + + +def calculate_acceleration_vector(velocity,pose): + radius = 6 + tether_factor = 10 + #tether_factor = 1 + #calculate the velocity, not including vertical component + xy_velocity = np.sqrt(velocity.x*velocity.x+velocity.y*velocity.y) + #calculate the centripetal acceleration needed to keep it moving in a circle + centripetal_acceleration = xy_velocity*xy_velocity/radius + #set the forward acceleration to -1m/s^2 if the drone is moving faster than 5m/s, and to 1m/s^2 if the drone is moving slower than 5m/s + forward_acceleration = 0 + if(xy_velocity-3!=0): + forward_acceleration = np.abs(xy_velocity-3)/(3-xy_velocity)/2 + angle_velocity = np.arctan(velocity.y/velocity.x) + if(velocity.x<0): + angle_velocity+=np.pi + angle_centripetal_acceleration=angle_velocity+np.pi/2 + acceleration_x=np.cos(angle_velocity)*forward_acceleration+np.cos(angle_centripetal_acceleration)*centripetal_acceleration + acceleration_y=np.sin(angle_velocity)*forward_acceleration+np.sin(angle_centripetal_acceleration)*centripetal_acceleration + + if pose.z>5: + acceleration_z = -1*abs(velocity.z+1)/(velocity.z+1) + elif pose.z<4: + acceleration_z = abs(velocity.z-1)/(1-velocity.z) + else: + acceleration_z = -1*(np.sign(velocity.z)) + return acceleration_x, acceleration_y, tether_factor*acceleration_z + + +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 = 5 + 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.z<4.5 or velocity.x>1 or velocity.y>1): + if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)): + rospy.logwarn('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 + + setpointpub.publish(initial_pose) + + while not rospy.is_shutdown(): + x_accel, y_accel, z_accel=calculate_acceleration_vector(velocity,pose) + to_publish=Vector3Stamped() + to_publish.vector.x = x_accel + to_publish.vector.y=y_accel + to_publish.vector.z=z_accel + pub.publish(to_publish) + rate.sleep() + +if __name__ == '__main__': + pose = None + velocity = None + 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 + + + + diff --git a/src/turtle_test.py b/src/turtle_test.py index e592326..c6a1f25 100755 --- a/src/turtle_test.py +++ b/src/turtle_test.py @@ -3,7 +3,6 @@ import rospy from turtlesim.srv import * from geometry_msgs.msg import PoseStamped - def callback(data): teleport_turtle(data.pose.position.x/2+6,data.pose.position.y/2+6,0)