-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgeneradorlecturasparaHMM_takeshi_Pose.py
88 lines (75 loc) · 3.77 KB
/
generadorlecturasparaHMM_takeshi_Pose.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#!/usr/bin/env python2
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 17 22:54:18 2019
@author: oscar
"""
from geometry_msgs.msg import Quaternion
import numpy as np
import rospy
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped
from tf.transformations import *
from tf.msg import tfMessage
from tf.transformations import euler_from_quaternion
def callback(laser,pose):
#######################################################
#centroides = np.load('centroidesVk.npy')
lec=np.asarray(laser.ranges)
#print (lec.shape)
lec[np.isinf(lec)]=13.5
lec_str=str(lec[0])+','
for i in range (1,len(lec)):
lec_str=lec_str+str(lec[i])+','
#print (lec_str)
lec.reshape(len(laser.ranges),1 )
print (pose.pose)
#symbol= np.power(lec.T-centroides,2).sum(axis=1,keepdims=True).argmax()
############################################################
#ccxyth=np.load('HMM/ccxyth.npy')#xy = np.load('ccxy.npy')
#cc= ccxyth[:,1:]
"""xy = np.asarray((odometria.pose.pose.position.x,odometria.pose.pose.position.y))
quaternion = (
odometria.pose.pose.orientation.x,
odometria.pose.pose.orientation.y,
odometria.pose.pose.orientation.z,
odometria.pose.pose.orientation.w)
euler = euler_from_quaternion(quaternion)
#roll = euler[0]
#pitch = euler[1]
#yaw = euler[2]
xyth= np.asarray((odometria.pose.pose.position.x,odometria.pose.pose.position.y,euler[2]))
print(str(xyth[0])+","+ str(xyth[1]) +","+str(euler[2]) +' \n')
#symbolxy =np.power(xy-ccxy,2).sum(axis=1 ,keepdims=True).argmax()
#symbolxy =np.power(xyth-ccxyth,2).sum(axis=1 ,keepdims=True).argmax()
#lecodom=np.asarray((xyth.pose.pose.position.x , xyth.pose.pose.position.y, xyth.twist.twist.angular.z).reshape(1,3)
# lecodom= np.power(xyth[0:2]-centxyth[:,0:2],2).sum(axis=1,keepdims=True).argmax()
#velocidad= np.asarray((twist.linear.x,twist.angular.z))
#print(lec_str+str(xyth[0])+","+ str(xyth[1]) +","+str(euler[2]) +","+ str(symbol)+','+str(symbolxy)+','+str(velocidad[0])+','+str(velocidad[1])+' \n' )
with open('lecturasconodometria.txt' , 'a') as out:
out.write (lec_str +str(xyth[0])+","+ str(xyth[1]) +","+str(euler[2]) +'\n' )
#out.write (lec_str +str(xyth[0])+","+ str(xyth[1]) +","+str(euler[2]) +","+ str(symbol)+','+str(symbolxy)+'\n' )
"""
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
#twist = message_filters.Subscriber('cmd_vel',Twist)
symbol= message_filters.Subscriber('/hsrb/base_scan',LaserScan)
#pose = message_filters.Subscriber('/navigation/localization/amcl_pose',PoseWithCovarianceStamped)#TAKESHI REAL
pose = message_filters.Subscriber('/hsrb/base_pose',PoseStamped)#TAKESHI GAZEBO
#ats= message_filters.ApproximateTimeSynchronizer([symbol,odom,twist],queue_size=5,slop=.1,allow_headerless=True)
ats= message_filters.ApproximateTimeSynchronizer([symbol,pose],queue_size=5,slop=.1,allow_headerless=True)
ats.registerCallback(callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()