Merge branch 'master' of https://git.spirirobotics.com/scorpio1/OscillationControlWithCircle

merge to add read me

launch/spiri_circle_acceleration.launch

@@ -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="circle_acceleration.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>

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
+
+
+ 
+

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)