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)