- packet timeout in broadcastPing() overflow bug fixed.

- bug in python removeStuffing() fixed.

Author: ROBOTIS-zerom

c++/src/dynamixel_sdk/protocol2_packet_handler.cpp

@@ -506,6 +506,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t
   uint8_t txpacket[10]        = {0};
   uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0};
 
+  double tx_time_per_byte = (1000.0 / (double)port->getBaudRate()) * 10.0;
+
   txpacket[PKT_ID]            = BROADCAST_ID;
   txpacket[PKT_LENGTH_L]      = 3;
   txpacket[PKT_LENGTH_H]      = 0;
@@ -519,7 +521,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t
   }
 
   // set rx timeout
-  port->setPacketTimeout((uint16_t)(wait_length * 30));
+  //port->setPacketTimeout((uint16_t)(wait_length * 30));
+  port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0);
 
   while(1)
   {

c/src/dynamixel_sdk/protocol2_packet_handler.c

@@ -577,6 +577,8 @@ void broadcastPing2(int port_num)
   uint16_t rx_length = 0;
   uint16_t wait_length = STATUS_LENGTH * MAX_ID;
 
+  double tx_time_per_byte = (1000.0 / (double)getBaudRate(port_num)) * 10.0;
+
   packetData[port_num].broadcast_ping_id_list = (uint8_t *)calloc(255, sizeof(uint8_t));
   if (packetData[port_num].broadcast_ping_id_list == NULL)
   {
@@ -610,7 +612,8 @@ void broadcastPing2(int port_num)
   }
 
   // set rx timeout
-  setPacketTimeout(port_num, (uint16_t)(wait_length * 30));
+  //setPacketTimeout(port_num, (uint16_t)(wait_length * 30));
+  setPacketTimeoutMSec(port_num, ((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0);
 
   while (1)
   {

python/src/dynamixel_sdk/protocol2_packet_handler.py

@@ -21,8 +21,8 @@
 
 from .robotis_def import *
 
-TXPACKET_MAX_LEN = 4 * 1024
-RXPACKET_MAX_LEN = 4 * 1024
+TXPACKET_MAX_LEN = 1 * 1024
+RXPACKET_MAX_LEN = 1 * 1024
 
 # for Protocol 2.0 Packet
 PKT_HEADER0 = 0
@@ -383,6 +383,8 @@ def broadcastPing(self, port):
         txpacket = [0] * 10
         rxpacket = []
 
+        tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0;
+
         txpacket[PKT_ID] = BROADCAST_ID
         txpacket[PKT_LENGTH_L] = 3
         txpacket[PKT_LENGTH_H] = 0
@@ -394,7 +396,8 @@ def broadcastPing(self, port):
             return data_list, result
 
         # set rx timeout
-        port.setPacketTimeout(wait_length * 1)
+        #port.setPacketTimeout(wait_length * 1)
+        port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0);
 
         while True:
             rxpacket += port.readPort(wait_length - rx_length)

ros/src/dynamixel_sdk/protocol2_packet_handler.cpp

@@ -506,6 +506,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t
   uint8_t txpacket[10]        = {0};
   uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0};
 
+  double tx_time_per_byte = (1000.0 / (double)port->getBaudRate()) * 10.0;
+
   txpacket[PKT_ID]            = BROADCAST_ID;
   txpacket[PKT_LENGTH_L]      = 3;
   txpacket[PKT_LENGTH_H]      = 0;
@@ -519,7 +521,8 @@ int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t
   }
 
   // set rx timeout
-  port->setPacketTimeout((uint16_t)(wait_length * 30));
+  //port->setPacketTimeout((uint16_t)(wait_length * 30));
+  port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0);
 
   while(1)
   {

ros/src/dynamixel_sdk/protocol2_packet_handler.py

@@ -197,10 +197,9 @@ def removeStuffing(self, packet):
                     packet[i + PKT_INSTRUCTION - 1] == 0xFF) and (packet[i + PKT_INSTRUCTION - 2] == 0xFF):
                 # FF FF FD FD
                 packet_length_out = packet_length_out - 1
-                i += 1
-
-            packet[index] = packet[i + PKT_INSTRUCTION]
-            index += 1
+            else:
+                packet[index] = packet[i + PKT_INSTRUCTION]
+                index += 1
 
         packet[index] = packet[PKT_INSTRUCTION + packet_length_in - 2]
         packet[index + 1] = packet[PKT_INSTRUCTION + packet_length_in - 1]
@@ -384,6 +383,8 @@ def broadcastPing(self, port):
         txpacket = [0] * 10
         rxpacket = []
 
+        tx_time_per_byte = (1000.0 / port.getBaudRate()) *10.0;
+
         txpacket[PKT_ID] = BROADCAST_ID
         txpacket[PKT_LENGTH_L] = 3
         txpacket[PKT_LENGTH_H] = 0
@@ -395,7 +396,8 @@ def broadcastPing(self, port):
             return data_list, result
 
         # set rx timeout
-        port.setPacketTimeout(wait_length * 1)
+        #port.setPacketTimeout(wait_length * 1)
+        port.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * MAX_ID) + 16.0);
 
         while True:
             rxpacket += port.readPort(wait_length - rx_length)