def move(msg, arg): #Set rospy rate rate = rospy.Rate(10) #Expose variables from subscriber line goal = arg[0] zumy = arg[1] #Creating a new current state based on the information from Haoyu's code current_state = {'x': msg.position.x, 'y': msg.position.y, 'theta': msg.position.theta} #Plugging the information from Haoyu's code into Vijay's getCmdVel function to calculate v_x and omega_z vel = get_vel.getCmdVel(current_state, goal) #Creating the ability to publish to the zumy zumy_vel = rospy.Publisher('/%s/cmd_vel' % zumy, Twist, queue_size=2) #Creating the message type to publish to the zumy (information from vel) cmd = Twist() cmd.linear.x = vel['lin_x'] cmd.linear.y = 0 cmd.linear.z = 0 cmd.angular.x = 0 cmd.angular.y = 0 cmd.angular.z = vel['ang_z'] #Publish new velocity information to the zumy zumy_vel.publish(cmd) print cmd.linear.x print cmd.angular.z rate.sleep()
def move(self, request): #Set rospy rate #Expose variables from subscriber line self.goal = request.goal self.goal_flag = False self.counter = 0 cmd = Twist() cmd.linear.y = 0 cmd.linear.z = 0 cmd.angular.x = 0 cmd.angular.y = 0 #Creating a new current state based on the information from Haoyu's code #Plugging the information from Haoyu's code into Vijay's getCmdVel function to calculate v_x and omega_z while self.counter<5: #self.pubTFPos() (vel, self.goal_flag) = get_vel.getCmdVel(self.position, self.goal) #Creating the ability to publish to the zumy #Creating the message type to publish to the zumy (information from vel) if not self.goal_flag: cmd.angular.z = vel['ang_z'] cmd.linear.x = vel['lin_x'] self.counter = 0 else: cmd.angular.z = 0 cmd.linear.x = 0 self.counter = self.counter + 1 #Publish new velocity information to the zumy self.vel_pub.publish(cmd) print cmd.linear.x print cmd.angular.z self.rate.sleep() return True
def move(msg, arg): #Set rospy rate rate = rospy.Rate(10) #Expose variables from subscriber line zumy1 = arg[0] zumy1_tag = str('ar_marker_') + str(arg[1]) goal1 = arg[2] zumy2 = arg[3] zumy2_tag = str('ar_marker_') + str(arg[4]) goal2 = arg[5] #Allow the current state dictionaries to be treated as global variables within the callback function global current_state1 global current_state2 #Always update the current states of the Zumys if msg.zumyID == zumy1_tag: #Creating a new current state based on the information from Haoyu's code current_state1 = {'x': msg.position.x, 'y': msg.position.y, 'theta': msg.position.theta} elif msg.zumyID == zumy2_tag: #Creating a new current state based on the information from Haoyu's code current_state2 = {'x': msg.position.x, 'y': msg.position.y, 'theta': msg.position.theta} #Create the ability to publish to the Zumys zumy_vel1 = rospy.Publisher('/%s/cmd_vel' % zumy1, Twist, queue_size=2) zumy_vel2 = rospy.Publisher('/%s/cmd_vel' % zumy2, Twist, queue_size=2) #Creating the message type to publish to the Zumys and defining constant parameters cmd1 = Twist() cmd1.linear.y = 0 cmd1.linear.z = 0 cmd1.angular.x = 0 cmd1.angular.y = 0 cmd2 = Twist() cmd2.linear.y = 0 cmd2.linear.z = 0 cmd2.angular.x = 0 cmd2.angular.y = 0 #Determine what to do based on whether or not the Zumys will collide if not collide(current_state1, current_state2, .5): #Determine what state information is currently being sent across Haoyu's topic if msg.zumyID == zumy1_tag: print 'ZUMY1' #Plugging the information from Haoyu's code into Vijay's getCmdVel function to calculate v_x and omega_z vel1 = get_vel.getCmdVel(current_state1, goal1) #Update the message type to publish to zumy1 cmd1.linear.x = vel1['lin_x'] cmd1.angular.z = vel1['ang_z'] #Publish new velocity information to zumy2 zumy_vel1.publish(cmd1) rate.sleep() elif msg.zumyID == zumy2_tag: print 'ZUMY2' #Plugging the information from Haoyu's code into Vijay's getCmdVel function to calculate v_x and omega_z vel2 = get_vel.getCmdVel(current_state2, goal2) #Creating the message type to publish to zumy2 cmd2.linear.x = vel2['lin_x'] cmd2.angular.z = vel2['ang_z'] #Publish new velocity information to zumy2 zumy_vel2.publish(cmd2) # rate.sleep() elif collide(current_state1, current_state2, .5): print 'TOO CLOSE!' #Define the time when a collision was first detected collision_detection_time = time.time() #Initialize a current time current_time = time.time() while current_time - collision_detection_time < 2: #Redefine the current time variable current_time = time.time() #Hold Zumy1 temorarily cmd1.linear.x = 0 cmd1.angular.z = 0 #Back Zumy2 off cmd2.linear.x = -.15 cmd2.angular.z = 0 #Publish collision-avoidance twists to Zumys if they get too close zumy_vel1.publish(cmd1) zumy_vel2.publish(cmd2) rate.sleep() print 'IN LOOP!'