launch/spiri_circle_acceleration.launch

@@ -1,54 +0,0 @@
-<?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

@@ -1,123 +0,0 @@
-#!/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,6 +3,7 @@ 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)