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!'
