class DualDAE_node: def __init__(self,settings): print("loading model...") self.dae1 = Model(settings.model1) self.dae2 = None if hasattr(settings,'model2'): self.dae2 = Model(settings.model2) self.new_goal = False self.message_sent = False self.goals = GoalManager(settings) self.laser1 = None self.goal1 = None self.command1 = None in_count = 0 if self.dae1.model.mmdae_type[0]: self.laser1 = SequenceInput(self.dae1.model.dims[in_count],self.dae1.model.mean[in_count],self.dae1.model.std[in_count],self.dae1.model.sequence,self.dae1.model.normalise[in_count]) in_count += 1 if self.dae1.model.mmdae_type[1]: self.goal1 = SequenceInput(self.dae1.model.dims[in_count],self.dae1.model.mean[in_count],self.dae1.model.std[in_count],self.dae1.model.sequence,self.dae1.model.normalise[in_count]) in_count += 1 if self.dae1.model.mmdae_type[2]: self.command1 = SequenceInput(self.dae1.model.dims[in_count],self.dae1.model.mean[in_count],self.dae1.model.std[in_count],self.dae1.model.sequence,self.dae1.model.normalise[in_count]) in_count += 1 self.laser2 = None self.goal2 = None self.command2 = None if self.dae2 is not None: in_count = 0 if self.dae2.model.mmdae_type[0]: self.laser2 = SequenceInput(self.dae2.model.dims[in_count],self.dae2.model.mean[in_count],self.dae2.model.std[in_count],self.dae2.model.sequence,self.dae2.model.normalise[in_count]) in_count += 1 if self.dae2.model.mmdae_type[1]: self.goal2 = SequenceInput(self.dae2.model.dims[in_count],self.dae2.model.mean[in_count],self.dae2.model.std[in_count],self.dae2.model.sequence,self.dae2.model.normalise[in_count]) in_count += 1 if self.dae2.model.mmdae_type[2]: self.command2 = SequenceInput(self.dae2.model.dims[in_count],self.dae2.model.mean[in_count],self.dae2.model.std[in_count],self.dae2.model.sequence,self.dae2.model.normalise[in_count]) in_count += 1 print("...done.") self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.goal_pub = rospy.Publisher("/goal_reached", String, queue_size=10) self.acc_pub = rospy.Publisher("/accuracy",String, queue_size=10) self.data_pub = rospy.Publisher('/data', Float64MultiArray,queue_size=10) self.scan_sub = rospy.Subscriber(settings.topic, numpy_msg(LaserScan), self.laser_callback) self.goal_sub = rospy.Subscriber("/goal", String, self.goal_callback) self.pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.pose_callback) self.stall_sub = rospy.Subscriber("/stall", Bool, self.stall_callback) self.twist_msg = Twist() self.string_msg = String() self.data_msg = Float64MultiArray() print("\nAwaiting first goal!") time.sleep(2) self.string_msg.data = 's' self.goal_pub.publish(self.string_msg) self.string_msg.data = '' def stall_callback(self,stall): if stall.data: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = 'c' self.goal_pub.publish(self.string_msg) print 'Collision!' rospy.signal_shutdown('Collision') sys.exit("Collision") def pose_callback(self,pose): if self.goal1 is None and self.goal2 is None: return # Convert pose to x,y,theta (from quaternion orientation) quaternion = N.asarray((pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w)) pose = N.asarray([pose.pose.pose.position.x, pose.pose.pose.position.y,efq(quaternion)[2]]) self.goals.update_pose(pose) if self.goals.next_goal_flag: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = str(self.goals.current_goal) self.goal_pub.publish(self.string_msg) def laser_callback(self,laser): if self.goal1 is not None or self.goal2 is not None: # Check to see if first goal received if not self.goals.first_goal: return # Shift goal sequence and add current relative goal self.goals.update_relative() # Process laser measurement if self.laser1 is not None: self.laser1.update(laser.ranges) if self.laser2 is not None: self.laser2.update(laser.ranges) # Process goal measurement if self.goal1 is not None: self.goal1.update(self.goals.relative_goal) if self.goal2 is not None: self.goal2.update(self.goals.relative_goal) x1, z1 = self.dae1.process(self.laser1,self.goal1,self.command1) x2,z2 = [0,0] factor = 1.0 if self.dae2 is not None: factor = laser.ranges.min() if laser.ranges.min() < 1.0 else 1.0 factor = factor if factor > 0.5 else 0.5 factor = 8*factor - 4 factor = tanh(factor) x2, z2 = self.dae2.process(self.laser2,self.goal2,self.command2) self.twist_msg.linear.x = factor*x1 + (1.0-factor)*x2 self.twist_msg.angular.z = factor*z1 + (1.0-factor)*z2 # Publish command self.cmd_pub.publish(self.twist_msg) # Print information sys.stdout.write(' \r') sys.stdout.write('Goal:\tx = %2.1f, y = %2.1f\t\tTo Goal:\tr = %2.1f, theta = %2.1f\tUsing %s\r\r' % (self.goals.current_goal[0],self.goals.current_goal[1],self.goals.relative_goal[0],self.goals.relative_goal[1],factor)) sys.stdout.flush() def goal_callback(self,goal): # if 's' (end reached) or 'c' (collision) if goal.data == 's': rospy.signal_shutdown('goal reached') sys.exit("Goal Reached") if goal.data == 'c': rospy.signal_shutdown('Collision') sys.exit('Collision') # receive new goal self.goals.update_goal(N.asarray([float(goal.data.split(',')[0]),float(goal.data.split(',')[1])])) # dodgy screen update sys.stdout.write(' \r') sys.stdout.flush()
class CNN_node: def __init__(self): print("loading model...") self.cnn = Model() self.new_goal = False self.message_sent = False self.goals = GoalManager() self.laser = None self.goal = None self.command = None self.pose = None print("...done.") self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.goal_pub = rospy.Publisher("/goal_reached", String, queue_size=10) self.acc_pub = rospy.Publisher("/accuracy",String, queue_size=10) self.data_pub = rospy.Publisher('/data', Float64MultiArray,queue_size=10) self.scan_sub = rospy.Subscriber("/base_scan", numpy_msg(LaserScan), self.laser_callback) self.goal_sub = rospy.Subscriber("/move_base/goal", MoveBaseActionGoal, self.goal_callback) self.pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.pose_callback) self.stall_sub = rospy.Subscriber("/stall", Bool, self.stall_callback) self.twist_msg = Twist() self.string_msg = String() self.data_msg = Float64MultiArray() print("\nAwaiting first goal!") time.sleep(2) self.string_msg.data = 's' self.goal_pub.publish(self.string_msg) self.string_msg.data = '' def stall_callback(self,stall): if stall.data: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = 'c' self.goal_pub.publish(self.string_msg) print 'Collision!' rospy.signal_shutdown('Collision') sys.exit("Collision") def pose_callback(self,pose): self.pose = N.asarray([pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w]) self.goals.update_pose(self.pose) if self.goals.next_goal_flag: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = str(self.goals.current_goal) self.goal_pub.publish(self.string_msg) def laser_callback(self,laser): if self.goal is not None: if not self.goals.first_goal: return # Shift goal sequence and add current relative goal self.goals.update_relative() las = laser.ranges self.laserdata = N.asarray(las) if laser is not None and self.goal is not None: out = self.cnn.run(self.laserdata, self.goals.goaldata) self.twist_msg.linear.x = out[0][0] self.twist_msg.angular.z = out[0][1] # Publish command if not self.goals.next_goal_flag: self.cmd_pub.publish(self.twist_msg) # Print information sys.stdout.write(' \r') #sys.stdout.write('Goal:\tx = %2.1f, y = %2.1f\t\tTo Goal:\tr = %2.1f, theta = %2.1f\tUsing %s\r\r' % (self.goals.current_goal[0],self.goals.current_goal[1],self.goals.relative_goal[0],self.goals.relative_goal[1],factor)) sys.stdout.flush() def goal_callback(self,goalPose): # receive new goal self.goals.update_goal(N.asarray([goalPose.goal.target_pose.pose.position.x, goalPose.goal.target_pose.pose.position.y, goalPose.goal.target_pose.pose.orientation.x, goalPose.goal.target_pose.pose.orientation.y, goalPose.goal.target_pose.pose.orientation.z, goalPose.goal.target_pose.pose.orientation.w])) self.goal = goalPose # dodgy screen update sys.stdout.write('\r') sys.stdout.flush()
class DualDAE_node: def __init__(self, settings): print("loading model...") self.dae1 = Model(settings.model1) self.dae2 = None if hasattr(settings, 'model2'): self.dae2 = Model(settings.model2) self.new_goal = False self.message_sent = False self.goals = GoalManager(settings) self.laser1 = None self.goal1 = None self.command1 = None in_count = 0 if self.dae1.model.mmdae_type[0]: self.laser1 = SequenceInput(self.dae1.model.dims[in_count], self.dae1.model.mean[in_count], self.dae1.model.std[in_count], self.dae1.model.sequence, self.dae1.model.normalise[in_count]) in_count += 1 if self.dae1.model.mmdae_type[1]: self.goal1 = SequenceInput(self.dae1.model.dims[in_count], self.dae1.model.mean[in_count], self.dae1.model.std[in_count], self.dae1.model.sequence, self.dae1.model.normalise[in_count]) in_count += 1 if self.dae1.model.mmdae_type[2]: self.command1 = SequenceInput(self.dae1.model.dims[in_count], self.dae1.model.mean[in_count], self.dae1.model.std[in_count], self.dae1.model.sequence, self.dae1.model.normalise[in_count]) in_count += 1 self.laser2 = None self.goal2 = None self.command2 = None if self.dae2 is not None: in_count = 0 if self.dae2.model.mmdae_type[0]: self.laser2 = SequenceInput( self.dae2.model.dims[in_count], self.dae2.model.mean[in_count], self.dae2.model.std[in_count], self.dae2.model.sequence, self.dae2.model.normalise[in_count]) in_count += 1 if self.dae2.model.mmdae_type[1]: self.goal2 = SequenceInput(self.dae2.model.dims[in_count], self.dae2.model.mean[in_count], self.dae2.model.std[in_count], self.dae2.model.sequence, self.dae2.model.normalise[in_count]) in_count += 1 if self.dae2.model.mmdae_type[2]: self.command2 = SequenceInput( self.dae2.model.dims[in_count], self.dae2.model.mean[in_count], self.dae2.model.std[in_count], self.dae2.model.sequence, self.dae2.model.normalise[in_count]) in_count += 1 print("...done.") self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.goal_pub = rospy.Publisher("/goal_reached", String, queue_size=10) self.acc_pub = rospy.Publisher("/accuracy", String, queue_size=10) self.data_pub = rospy.Publisher('/data', Float64MultiArray, queue_size=10) self.scan_sub = rospy.Subscriber(settings.topic, numpy_msg(LaserScan), self.laser_callback) self.goal_sub = rospy.Subscriber("/goal", String, self.goal_callback) self.pose_sub = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.pose_callback) self.stall_sub = rospy.Subscriber("/stall", Bool, self.stall_callback) self.twist_msg = Twist() self.string_msg = String() self.data_msg = Float64MultiArray() print("\nAwaiting first goal!") time.sleep(2) self.string_msg.data = 's' self.goal_pub.publish(self.string_msg) self.string_msg.data = '' def stall_callback(self, stall): if stall.data: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = 'c' self.goal_pub.publish(self.string_msg) print 'Collision!' rospy.signal_shutdown('Collision') sys.exit("Collision") def pose_callback(self, pose): if self.goal1 is None and self.goal2 is None: return # Convert pose to x,y,theta (from quaternion orientation) quaternion = N.asarray( (pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w)) pose = N.asarray([ pose.pose.pose.position.x, pose.pose.pose.position.y, efq(quaternion)[2] ]) self.goals.update_pose(pose) if self.goals.next_goal_flag: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = str(self.goals.current_goal) self.goal_pub.publish(self.string_msg) def laser_callback(self, laser): if self.goal1 is not None or self.goal2 is not None: # Check to see if first goal received if not self.goals.first_goal: return # Shift goal sequence and add current relative goal self.goals.update_relative() # Process laser measurement if self.laser1 is not None: self.laser1.update(laser.ranges) if self.laser2 is not None: self.laser2.update(laser.ranges) # Process goal measurement if self.goal1 is not None: self.goal1.update(self.goals.relative_goal) if self.goal2 is not None: self.goal2.update(self.goals.relative_goal) x1, z1 = self.dae1.process(self.laser1, self.goal1, self.command1) x2, z2 = [0, 0] factor = 1.0 if self.dae2 is not None: factor = laser.ranges.min() if laser.ranges.min() < 1.0 else 1.0 factor = factor if factor > 0.5 else 0.5 factor = 8 * factor - 4 factor = tanh(factor) x2, z2 = self.dae2.process(self.laser2, self.goal2, self.command2) self.twist_msg.linear.x = factor * x1 + (1.0 - factor) * x2 self.twist_msg.angular.z = factor * z1 + (1.0 - factor) * z2 # Publish command self.cmd_pub.publish(self.twist_msg) # Print information sys.stdout.write( ' \r' ) sys.stdout.write( 'Goal:\tx = %2.1f, y = %2.1f\t\tTo Goal:\tr = %2.1f, theta = %2.1f\tUsing %s\r\r' % (self.goals.current_goal[0], self.goals.current_goal[1], self.goals.relative_goal[0], self.goals.relative_goal[1], factor)) sys.stdout.flush() def goal_callback(self, goal): # if 's' (end reached) or 'c' (collision) if goal.data == 's': rospy.signal_shutdown('goal reached') sys.exit("Goal Reached") if goal.data == 'c': rospy.signal_shutdown('Collision') sys.exit('Collision') # receive new goal self.goals.update_goal( N.asarray([ float(goal.data.split(',')[0]), float(goal.data.split(',')[1]) ])) # dodgy screen update sys.stdout.write( ' \r' ) sys.stdout.flush()