def __init__(self):
self.sleep_time = 1 / 5
# +z is down for the stewart platform -- the x360.yaml denotes this with a z mask of -1
self.lambda_matrix = {'x':0.5,'y':-0.5,'z':-0.5,'yaw':0.0} # tuning for amount and direction
self.pelican_ctrl_pub = rospy.Publisher('/Pelican/ctrl', mav_ctrl)
self.pelican_ctrl = mav_ctrl()
# velocity with default parameters (should be safe)
self.pelican_ctrl.type = 2 # 1--acceleration 2--velocity 3--position
self.pelican_ctrl.x = 0
self.pelican_ctrl.y = 0
self.pelican_ctrl.z = 0
self.pelican_ctrl.yaw = 0
self.pelican_ctrl.v_max_xy = 1
self.pelican_ctrl.v_max_z = 1
self.pelican_safe = mav_ctrl()
self.pelican_safe.type = 2 # 1--acceleration 2--velocity 3--position
self.pelican_safe.x = 0
self.pelican_safe.y = 0
self.pelican_safe.z = 0
self.pelican_safe.yaw = 0
self.pelican_safe.v_max_xy = 1
self.pelican_safe.v_max_z = 1
self.safeIsWritten = False
rospy.init_node('pelican_control', anonymous=True)
self.joy_data = None
rospy.Subscriber('/Pelican/Joy', Joy, self.controllerCallback)
# don't have a proper node name for this yet but nothing is implemented yet
self.pelican_pose = TransformStamped()
rospy.Subscriber('/Pelican/PoseFeedback', TransformStamped, self.poseCallback) # rename everything to transforms
self.pelican_safe_position = [[-1.5,1],[-1,1],[0,2]]
self.stewartDefaultTwist = Twist() # the default twist
self.stewartDefaultTwist.linear.z=0.21
# even if the IK doesn't solve this twist it is still this far away from the target. It may actually turn out to help get the system unstuck
rospy.Subscriber('/Pelican/StewartTwist', Twist, self.stewartTwistCallback)
self.stewartCurrentTwist=None
def callback(data):
angles = tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z,data.pose.orientation.w])
msg = mav_ctrl()
msg.header = data.header
msg.type = mav_ctrl.position
msg.x = data.pose.position.x
msg.y = data.pose.position.y
msg.z = data.pose.position.z
msg.yaw = angles[2]
msg.v_max_xy = -1 #hardcoded, refer to asctec files
msg.v_max_z = -1
pub.publish(msg)
