diff --git a/benchmark_main.go b/benchmark_main.go
index 8c703aa2c..e3e6d3f41 100644
--- a/benchmark_main.go
+++ b/benchmark_main.go
@@ -99,11 +99,6 @@ func main() {
 		go func() {
 			node.WaitForConsensusReady(consensus.ReadySignal)
 		}()
-	} else {
-		// Node waiting to add new block to the blockchain
-		go func() {
-			node.WaitForConsensusReady(consensus.ReadySignal)
-		}()
 	}
 
 	node.StartServer(*port)
diff --git a/consensus/consensus_leader.go b/consensus/consensus_leader.go
index 082d3efed..cf1663836 100644
--- a/consensus/consensus_leader.go
+++ b/consensus/consensus_leader.go
@@ -276,16 +276,19 @@ func (consensus *Consensus) processResponseMessage(payload []byte) {
 	_ = response
 	_ = signature
 
+
+	// proceed only when the message is not received before and this consensus phase is not done.
+
+	shouldProcess := true
+	consensus.mutex.Lock()
 	// check consensus Id
 	if consensusId != consensus.consensusId {
+		shouldProcess = false
 		consensus.Log.Debug("[ERROR] Received RESPONSE with wrong consensus Id", "myConsensusId", consensus.consensusId, "theirConsensusId", consensusId, "consensus", consensus)
-		return
 	}
 
-	// proceed only when the message is not received before and this consensus phase is not done.
-	consensus.mutex.Lock()
 	_, ok := consensus.responses[validatorId]
-	shouldProcess := !ok
+	shouldProcess = shouldProcess && !ok
 	if shouldProcess {
 		consensus.responses[validatorId] = validatorId
 		//consensus.Log.Debug("Number of responses received", "count", len(consensus.responses), "consensudId", consensusId)
diff --git a/node/node_handler.go b/node/node_handler.go
index 42ac23c3f..f1debde46 100644
--- a/node/node_handler.go
+++ b/node/node_handler.go
@@ -113,31 +113,44 @@ func (node *Node) WaitForConsensusReady(readySignal chan int) {
 	node.log.Debug("Waiting for consensus ready", "node", node)
 
 	var newBlock *blockchain.Block
+	timeoutCount := 0
 	for { // keep waiting for consensus ready
-		<-readySignal
+		retry := false
+		select {
+		case <-readySignal:
+		case <-time.After(8 * time.Second):
+			retry = true
+			timeoutCount++
+			node.log.Debug("Consensus timeout, retry!", "count", timeoutCount, "node", node)
+		}
+
 		//node.log.Debug("Adding new block", "currentChainSize", len(node.blockchain.Blocks), "numTxs", len(node.blockchain.GetLatestBlock().Transactions), "PrevHash", node.blockchain.GetLatestBlock().PrevBlockHash, "Hash", node.blockchain.GetLatestBlock().Hash)
-		for {
-			// Once we have more than 10 transactions pending we will try creating a new block
-			if len(node.pendingTransactions) >= 10 {
-				selectedTxs := node.getTransactionsForNewBlock()
-
-				if len(selectedTxs) == 0 {
-					node.log.Debug("No valid transactions exist", "pendingTx", len(node.pendingTransactions))
-				} else {
-					node.log.Debug("Creating new block", "numTxs", len(selectedTxs), "pendingTxs", len(node.pendingTransactions), "currentChainSize", len(node.blockchain.Blocks))
-
-					node.transactionInConsensus = selectedTxs
-					newBlock = blockchain.NewBlock(selectedTxs, node.blockchain.GetLatestBlock().Hash)
-					break
+		if !retry {
+			for {
+				// Once we have more than 10 transactions pending we will try creating a new block
+				if len(node.pendingTransactions) >= 10 {
+					selectedTxs := node.getTransactionsForNewBlock()
+
+					if len(selectedTxs) == 0 {
+						node.log.Debug("No valid transactions exist", "pendingTx", len(node.pendingTransactions))
+					} else {
+						node.log.Debug("Creating new block", "numTxs", len(selectedTxs), "pendingTxs", len(node.pendingTransactions), "currentChainSize", len(node.blockchain.Blocks))
+
+						node.transactionInConsensus = selectedTxs
+						newBlock = blockchain.NewBlock(selectedTxs, node.blockchain.GetLatestBlock().Hash)
+						break
+					}
 				}
+				// If not enough transactions to run consensus,
+				// periodically check whether we have enough transactions to package into block.
+				time.Sleep(1 * time.Second)
 			}
-			// If not enough transactions to run consensus,
-			// periodically check whether we have enough transactions to package into block.
 		}
 
 		// Send the new block to consensus so it can be confirmed.
-		node.BlockChannel <- *newBlock
-		time.Sleep(2 * time.Second)
+		if newBlock != nil {
+			node.BlockChannel <- *newBlock
+		}
 	}
 }