def __init__(self, anchors):
self.anchors = anchors
self.poseStampedPublisher = rospy.Publisher(util.topicName("marker_pose", "pose"), PoseStamped, queue_size=10)
self.markerArrayPublisher = rospy.Publisher(util.topicName("marker_pose", "markers"), PoseArray, queue_size=10)
self.poseSepPublisher = rospy.Publisher(util.topicName("marker_pose", "pose_sep"), PoseArray, queue_size=10)
rospy.Subscriber("cam2/camera/apriltags_marker", MarkerArray, self.markerCb)
self.cameraPose = PoseModel()
self.poses = []
def __init__(self):
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.localCb)
rospy.Subscriber("/itech_ros/mavros_offboard/enu", PoseStamped, self.enuCb)
self.localEnuPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "local_enu"), PoseStamped, queue_size=10)
self.offsetPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "offset"), PointStamped, queue_size=10)
self.offset = None
self.localPose = PoseModel()
self.localEnuPose = PoseModel()
#tick posee are the poses at the time that we get positioning data.
self.enuTickPose = PoseModel()
self.localTickPose = PoseModel()
self.r = euler_matrix(0, 0, math.pi/-2)
def __init__(self, mavModel):
self.cmdVelPub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)
self.bodyPub = rospy.Publisher(util.topicName("mavros_offboard", "body"), PoseStamped, queue_size=10)
self.enuPub = rospy.Publisher(util.topicName("mavros_offboard", "enu"), PoseStamped, queue_size=10)
self.destENUPub = rospy.Publisher(util.topicName("mavros_offboard", "dest_enu"), PoseStamped, queue_size=10)
self.safetyStatusPub = rospy.Publisher(
util.topicName("mavros_offboard", "safety_status"), String, queue_size=10
)
rospy.Subscriber("/itech_ros/marker_pose/pose", PoseStamped, self.cameraCb)
rospy.Subscriber(util.topicName("mavros_offboard", "set_destination"), PoseStamped, self.destCb)
rospy.Subscriber("/mavros/vfr_hud", VFR_HUD, self.hudCb)
rospy.Subscriber("/itech_ros/mavros_local_pose_fusion/local_enu", PoseStamped, self.localEnuFusionCb)
self.mavModel = mavModel
self.mavSafety = MavSafety(self.mavModel)
