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)