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)