class Anchor2TestCase(unittest.TestCase):
anchorData_ok = False
anchor2 = Anchor()
def callback(self, data):
self.anchorData_ok = True
self.anchor2 = data
# Test existance of topic
def test_if_anchor_2_is_published(self):
rospy.init_node('test_anchor_2')
rospy.Subscriber("/dwm1001/anchor2", Anchor, self.callback)
counter = 1
# give 5 seconds to check the topic is publishing something
while not rospy.is_shutdown() and counter < 5 and (
not self.anchorData_ok):
time.sleep(1)
counter += 1
rospy.loginfo("looping")
self.assertTrue(self.anchorData_ok)
# Test the id of Anchor if is a string
def test_if_anchor_2_id_is_string(self):
if isinstance(self.anchor2.id, str):
self.assertTrue(True)
else:
self.assertTrue(False)
# Test the x of Anchor if is a float
def test_if_anchor_2_x_is_float(self):
if isinstance(self.anchor2.x, float):
self.assertTrue(True)
else:
self.assertTrue(False)
# Test the y of Anchor if is a float
def test_if_anchor_2_y_is_float(self):
if isinstance(self.anchor2.y, float):
self.assertTrue(True)
else:
self.assertTrue(False)
# Test the z of Anchor if is a float
def test_if_anchor_2_z_is_float(self):
if isinstance(self.anchor2.z, float):
self.assertTrue(True)
else:
self.assertTrue(False)
# Test the z of Anchor if is a float
def test_if_anchor_2_distanceFromTag_is_float(self):
if isinstance(self.anchor2.distanceFromTag, float):
self.assertTrue(True)
else:
self.assertTrue(False)
def publishCoordinatesIntoTopics(self, networkDataArray):
"""
Publish anchors and tag in topics using Tag and Anchor Object
:param networkDataArray: Array from serial port containing all informations, tag xyz and anchor xyz
:returns: none
"""
# loop trough the array given by the serial port
for network in networkDataArray:
# check if there is any entry starting with AN, which means there is an anchor
if 'AN' in network:
# get the number after'AN' which we will use to pubblish topics, example /dwm1001/anchor1
temp_anchor_number = networkDataArray[networkDataArray.index(network)]
# construct the object for anchor(s)
anchor = Anchor( str(networkDataArray[networkDataArray.index(network) + 1]),
float(networkDataArray[networkDataArray.index(network) + 2]),
float(networkDataArray[networkDataArray.index(network) + 3]),
float(networkDataArray[networkDataArray.index(network) + 4]),
float(networkDataArray[networkDataArray.index(network) + 5]))
# publish each anchor, add anchor number to the topic, so we can pubblish multiple anchors
# example /dwm1001/anchor0, the last digit is taken from AN0 and so on
pub_anchor = rospy.Publisher('/dwm1001/anchor'+str(temp_anchor_number[-1]), Anchor, queue_size=1)
pub_anchor.publish(anchor)
rospy.logdebug("Anchor: "
+ str(anchor.id)
+ " x: "
+ str(anchor.x)
+ " y: "
+ str(anchor.y)
+ " z: "
+ str(anchor.z))
elif 'POS' in network:
# construct the object for the tag
tag = Tag(float(networkDataArray[networkDataArray.index(network) + 1]),
float(networkDataArray[networkDataArray.index(network) + 2]),
float(networkDataArray[networkDataArray.index(network) + 3]),)
# publish tag
pub_anchor = rospy.Publisher('/dwm1001/tag', Tag, queue_size=1)
pub_anchor.publish(tag)
rospy.logdebug("Tag: "
+ " x: "
+ str(tag.x)
+ " y: "
+ str(tag.y)
+ " z: "
+ str(tag.z))
def pubblishCoordinatesIntoTopics(self, networkDataArray):
"""
Publish anchors and tag in topics using Tag and Anchor Object
:param networkDataArray: Array from serial port containing all informations, tag xyz and anchor xyz
:returns: none
"""
# loop trough the array given by the serial port
for network in networkDataArray:
# check if there is any entry starting with AN, which means there is an anchor
if 'AN' in network:
# get the number after'AN' which we will use to pubblish topics, example /dwm1001/anchor1
temp_anchor_number = networkDataArray[networkDataArray.index(
network)]
# construct the object for anchor(s)
anchor = Anchor(
str(networkDataArray[networkDataArray.index(network) + 1]),
float(networkDataArray[networkDataArray.index(network) +
2]),
float(networkDataArray[networkDataArray.index(network) +
3]),
float(networkDataArray[networkDataArray.index(network) +
4]),
float(networkDataArray[networkDataArray.index(network) +
5]))
# publish each anchor, add anchor number to the topic, so we can pubblish multiple anchors
# example /dwm1001/anchor0, the last digit is taken from AN0 and so on
pub_anchor = rospy.Publisher('/dwm1001/anchor' +
str(temp_anchor_number[-1]),
Anchor,
queue_size=1)
pub_anchor.publish(anchor)
rospy.loginfo("Anchor: " + str(anchor.id) + " x: " +
str(anchor.x) + " y: " + str(anchor.y) + " z: " +
str(anchor.z))
elif 'POS' in network:
x_queue.append(
float(networkDataArray[networkDataArray.index(network) +
1]))
y_queue.append(
float(networkDataArray[networkDataArray.index(network) +
2]))
z_queue.append(
float(networkDataArray[networkDataArray.index(network) +
3]))
temp1 = len(x_queue)
# If the queue to be inputed into the filter isnt filled yet use non filtered values
if temp1 < 10:
# construct the object for the tag
tag = Tag(
float(
networkDataArray[networkDataArray.index(network) +
1]),
float(
networkDataArray[networkDataArray.index(network) +
2]),
float(
networkDataArray[networkDataArray.index(network) +
3]),
)
# Uses filtered values
else:
tag = Tag(
float(filter(x_queue, coefficient)),
float(filter(y_queue, coefficient)),
float(filter(z_queue, coefficient)),
)
# Removes the end of the queue to keep at size 10
x_queue.popleft()
y_queue.popleft()
z_queue.popleft()
# publish tag
pub_anchor = rospy.Publisher('/dwm1001/tag1',
Tag,
queue_size=1)
pub_anchor.publish(tag)
rospy.loginfo("Tag: " + " x: " + str(tag.x) + " y: " +
str(tag.y) + " z: " + str(tag.z))
__version__ = SYS_DEFS.VERSION
__maintainer__ = SYS_DEFS.MAINTAINER
__email__ = SYS_DEFS.EMAIL
__status__ = SYS_DEFS.STATUS
import rospy
from localizer_dwm1001.msg import Anchor
from localizer_dwm1001.msg import Tag
from localizer_dwm1001.srv import Anchor_0
from localizer_dwm1001.srv import Tag_srv
from localizer_dwm1001.srv import Anchor_1
from localizer_dwm1001.srv import Anchor_2
from localizer_dwm1001.srv import Anchor_3
from std_srvs.srv import Trigger, TriggerResponse
anchor_0 = Anchor()
anchor_1 = Anchor()
anchor_2 = Anchor()
anchor_3 = Anchor()
tag = Tag()
def triggerResponseAnchor0(request):
return (anchor_0.x, anchor_0.y, anchor_0.z)
def triggerResponseAnchor1(request):
return (anchor_1.x, anchor_1.y, anchor_1.z)
def triggerResponseAnchor2(request):
def pubblishCoordinatesIntoTopics(self, networkDataArray):
"""
Publish anchors and tag in topics using Tag and Anchor Object
:param networkDataArray: Array from serial port containing all informations, tag xyz and anchor xyz
:returns: none
"""
lastposx = 0.0
lastposy = 0.0
# loop trough the array given by the serial port
for network in networkDataArray:
# check if there is any entry starting with AN, which means there is an anchor
if 'AN' in network:
# get the number after'AN' which we will use to pubblish topics, example /dwm1001/anchor1
temp_anchor_number = networkDataArray[networkDataArray.index(
network)]
# construct the object for anchor(s)
anchor = Anchor(
str(networkDataArray[networkDataArray.index(network) + 1]),
float(networkDataArray[networkDataArray.index(network) +
2]),
float(networkDataArray[networkDataArray.index(network) +
3]),
float(networkDataArray[networkDataArray.index(network) +
4]),
float(networkDataArray[networkDataArray.index(network) +
5]))
# publish each anchor, add anchor number to the topic, so we can pubblish multiple anchors
# example /dwm1001/anchor0, the last digit is taken from AN0 and so on
pub_anchor = rospy.Publisher('/dwm1001/anchor' +
str(temp_anchor_number[-1]),
Anchor,
queue_size=1)
pub_anchor.publish(anchor)
rospy.loginfo("Anchor: " + str(anchor.id) + " x: " +
str(anchor.x) + " y: " + str(anchor.y) + " z: " +
str(anchor.z))
elif 'POS' in network:
# construct the object for the tag
tag = Tag(
float(networkDataArray[networkDataArray.index(network) +
1]),
float(networkDataArray[networkDataArray.index(network) +
2]),
float(networkDataArray[networkDataArray.index(network) +
3]),
)
#added by arun
tagpos = PoseStamped()
tagpos.header.frame_id = "map"
tagpos.header.stamp = rospy.Time.now()
tagpos.pose.position.x = float(
networkDataArray[networkDataArray.index(network) + 1])
tagpos.pose.position.y = float(
networkDataArray[networkDataArray.index(network) + 2])
tagpos.pose.position.z = float(
networkDataArray[networkDataArray.index(network) + 3])
yaw_ = atan2(tagpos.pose.position.x - lastposx,
tagpos.pose.position.y - lastposy)
# roll_ = 0
# pitch_ = 0
lastposx = tagpos.pose.position.x
lastposy = tagpos.pose.position.y
q = quaternion_from_euler(0.0, 0.0, yaw_) # yaw in radian
tagpos.pose.orientation = Quaternion(*q)
pub_anchor2 = rospy.Publisher('/dwm1001/tagpos',
PoseStamped,
queue_size=10)
#rospy.sleep(1)
pub_anchor2.publish(tagpos)
rospy.loginfo("tagpos: " + " position x: " +
str(tagpos.pose.position.x) + " position y: " +
str(tagpos.pose.position.y) + " position z: " +
str(tagpos.pose.position.z) +
" orientation x: " +
str(tagpos.pose.orientation.x) +
" orientation y: " +
str(tagpos.pose.orientation.y) +
" orientation z: " +
str(tagpos.pose.orientation.z) +
" orientation w: " +
str(tagpos.pose.orientation.w))
#added by arun end
# publish tag
pub_anchor = rospy.Publisher('/dwm1001/tag', Tag, queue_size=1)
#rospy.sleep(1)
pub_anchor.publish(tag)
rospy.loginfo("Tag: " + " x: " + str(tag.x) + " y: " +
str(tag.y) + " z: " + str(tag.z))