def get_odom(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def get_odom_angle(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
# Convert the rotation from a quaternion to an Euler angle
return quat_to_angle(Quaternion(*rot))
def model_stateCallback(self, data):
#see http://answers.ros.org/question/30227/turtlebot-autonomous-navigation-without-rviz/
#see http://answers.ros.org/question/9686/how-to-programatically-set-the-2d-pose-on-a-map/
#using http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
#using http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
#in order to get navigation working you need to publish 2 types of messages. a TF message and an Odomety message.
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space
#######################################################################################################################
#Here, we create a tf.TransformListener object.Once the listener is created, it starts receiving tf transformations
#over the wire, and buffers them for up to 10 seconds.
#self.listener = tf.TransformListener()
self.model_states_msg_cnt = (self.model_states_msg_cnt +
1) % self.Downsample_rate
if self.model_states_msg_cnt == 0:
timestamp = rospy.get_time()
strdata = str(data)
k = strdata.split(']')
k = k[0].split('[')
k = k[1].split(',')
indices = [
i for i, s in enumerate(k) if 'jackal' in s
] #finding the index of the jackal position in the header section
#Reading the file and extracting coordinates of the
#objects in the world
i = indices[0]
jackal_pose = data.pose[i]
trans = [
jackal_pose.position.x, jackal_pose.position.y,
jackal_pose.position.z
]
self.position = Point(*trans)
rot = [
jackal_pose.orientation.x, jackal_pose.orientation.y,
jackal_pose.orientation.z, jackal_pose.orientation.w
]
self.rotation = quat_to_angle(Quaternion(*rot))
#rospy.loginfo(self.position.x)
#rospy.loginfo(math.degrees(normalize_angle(self.rotation)))
try:
self.f1.write(str(timestamp))
self.f1.write('\n')
self.f1.write(str([self.position.x, self.position.y]))
self.f1.write('\n')
except:
pass
else:
return
def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
self.odometry_on=True
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
self.odometry_on=False
rospy.loginfo('odom')
rospy.loginfo(trans)
#rospy.loginfo(rot)
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
def model_stateCallback(self, data):
#rostopic echo /gazebo/model_states |grep "x: 5."
#rostopic echo /gazebo/model_states |grep 'x: 2[.]'
#see http://answers.ros.org/question/30227/turtlebot-autonomous-navigation-without-rviz/
#see http://answers.ros.org/question/9686/how-to-programatically-set-the-2d-pose-on-a-map/
#using http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
#using http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
#in order to get navigation working you need to publish 2 types of messages. a TF message and an Odomety message.
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space
#######################################################################################################################
#Splitting the test file and put in list
#http://stackoverflow.com/questions/11870399/python-regex-find-numbers-with-comma-in-string
#http://stackoverflow.com/questions/21744804/python-read-comma-separated-values-from-a-text-file-then-output-result-to-tex
#http://www.pythonforbeginners.com/dictionary/python-split
self.model_states_msg_cnt = (self.model_states_msg_cnt +
1) % self.Downsample_rate
if self.model_states_msg_cnt == 0:
strdata = str(data)
k = strdata.split(']')
k = k[0].split('[')
k = k[1].split(',')
indices = [
i for i, s in enumerate(k) if 'jackal' in s
] #finding the index of the jackal position in the header section
#Reading the file and extracting coordinates of the
#objects in the world
i = indices[0]
jackal_pose = data.pose[i]
trans = [
jackal_pose.position.x, jackal_pose.position.y,
jackal_pose.position.z
]
self.position = Point(*trans)
rot = [
jackal_pose.orientation.x, jackal_pose.orientation.y,
jackal_pose.orientation.z, jackal_pose.orientation.w
]
self.rotation = quat_to_angle(Quaternion(*rot))
#rospy.loginfo(self.position.x)
#rospy.loginfo(math.degrees(normalize_angle(self.rotation)))
else:
return
def publish_angular(self,cmd,rotation):
pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=5)
self.rotation=quat_to_angle(rotation)
angular_cmd=Twist()
#angular_cmd.angular.z=init_goal #twist angular
angular_cmd.angular.z=1 #twist angular
if cmd>pi:
self.init_goal=pi
self.sub_goal=cmd-2.0 * pi
while self.rotation<self.init_goal:
pub.publish(angular_cmd)
while self.rotation<self.sub_goal
pub.publish(angular_cmd)
return 'start'
else:
self.init_goal=cmd
self.sub_goal=0
while self.rotation<self.init_goal:
pub.publish(angular_cmd)
return 'start'
def link_stateCallback(self, data):
#see http://answers.ros.org/question/30227/turtlebot-autonomous-navigation-without-rviz/
#see http://answers.ros.org/question/9686/how-to-programatically-set-the-2d-pose-on-a-map/
#using http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
#using http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space:
#in order to get navigation working you need to publish 2 types of messages. a TF message and an Odomety message.
#The nav_msgs/Odometry message stores an estimate of the position and velocity of a robot in free space
#######################################################################################################################
#Here, we create a tf.TransformListener object.Once the listener is created, it starts receiving tf transformations
#over the wire, and buffers them for up to 10 seconds.
#self.listener = tf.TransformListener()
#try:
#timestamp=rospy.get_rostime()
#timestamp=rospy.get_time()
#rospy.loginfo(timestamp)
#Splitting the test file and put in list
#http://stackoverflow.com/questions/11870399/python-regex-find-numbers-with-comma-in-string
#http://stackoverflow.com/questions/21744804/python-read-comma-separated-values-from-a-text-file-then-output-result-to-tex
#http://www.pythonforbeginners.com/dictionary/python-split
strdata = str(data)
k = strdata.split(']')
k = k[0].split('[')
k = k[1].split(',')
indices = [
i for i, s in enumerate(k) if 'jackal::base_link' in s
] #finding the index of the jackal position in the header section
#Reading the file and extracting coordinates of the
#objects in the world
coordinates = [] #initialize list
rotation = []
lines = strdata.split(
'\n') #splitiing each line of the position/rotation data
#for i in xrange(0,len(lines)):
i = (indices[0]) * 10 + 3
if "position:" in lines[i]:
xline = lines[i + 1] #next line is "x"
[int(s) for s in xline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
x = s
yline = lines[i + 2] #next line is "y"
[int(s) for s in yline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
y = s
zline = lines[i + 3] #next line is "y"
[int(s) for s in zline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
z = s
coordinates.append((x, y, z)) #enter ney coordinate to list
trans = [
float(coordinates[0][0]),
float(coordinates[0][1]),
float(coordinates[0][2])
]
i = (indices[0]) * 10 + 7
if "orientation:" in lines[i]:
xline = lines[i + 1] #next line is "x"
[int(s) for s in xline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
xrot = s
yline = lines[i + 2] #next line is "y"
[int(s) for s in yline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
yrot = s
zline = lines[i + 3] #next line is "y"
[int(s) for s in zline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
zrot = s
wline = lines[i + 4] #next line is "y"
[int(s) for s in wline.split() if s.isdigit()
] #extract the numerical value of the coordinate out of rthe text
wrot = s
rotation.append(
(xrot, yrot, zrot, wrot)) #enter ney coordinate to list
rot = [
float(rotation[0][0]),
float(rotation[0][1]),
float(rotation[0][2]),
float(rotation[0][3])
]
#extracting the jackal's coordinates
#calling data saver function for keeping it both in list and text file
#for i in xrange(0,len(coordinates)):
#if i in indices:
#self.data_saver(coordinates[i])
#self.jackal_timestamps.append(timestamp)
#self.jackal_coordinates.append(coordinates[i])
# self.f1.write(str(timestamp))
# self.f1.write('\n')
# self.f1.write(str(coordinates[i]))
# self.f1.write('\n')
#rospy.loginfo('truth')
#rospy.loginfo(rot)
self.position = Point(*trans)
self.rotation = quat_to_angle(Quaternion(*rot))
rospy.loginfo(self.position.x)
