Fix bugs of roborts_base about buffer overflow. Force to respawn and …

…print ROS_INFO for roborts_base launch file.
RoboMaster · Mar 26, 2019 · 9a64670 · 9a64670
1 parent 75ced11
commit 9a64670
Show file tree

Hide file tree

Showing 10 changed files with 62 additions and 47 deletions.
diff --git a/roborts_base/chassis/chassis.cpp b/roborts_base/chassis/chassis.cpp
@@ -40,10 +40,11 @@ void Chassis::SDK_Init(){
   verison_client_->AsyncSendRequest(version,
                                     [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
                                                            roborts_sdk::cmd_version_id>::SharedFuture future) {
-                                      LOG_INFO << "Chassis Firmware Version: " << int(future.get()->version_id>>24&0xFF) <<"."
-                                               <<int(future.get()->version_id>>16&0xFF)<<"."
-                                               <<int(future.get()->version_id>>8&0xFF)<<"."
-                                               <<int(future.get()->version_id&0xFF);
+                                      ROS_INFO("Chassis Firmware Version: %d.%d.%d.%d",
+                                               int(future.get()->version_id>>24&0xFF),
+                                               int(future.get()->version_id>>16&0xFF),
+                                               int(future.get()->version_id>>8&0xFF),
+                                               int(future.get()->version_id&0xFF));
                                     });
 
   handle_->CreateSubscriber<roborts_sdk::cmd_chassis_info>(CHASSIS_CMD_SET, CMD_PUSH_CHASSIS_INFO,
@@ -59,15 +60,15 @@ void Chassis::SDK_Init(){
                                                                                     MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
 
   heartbeat_pub_ = handle_->CreatePublisher<roborts_sdk::cmd_heartbeat>(UNIVERSAL_CMD_SET, CMD_HEARTBEAT,
-                                                                           MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
+                                                                        MANIFOLD2_ADDRESS, CHASSIS_ADDRESS);
   heartbeat_thread_ = std::thread([this]{
-                                        roborts_sdk::cmd_heartbeat heartbeat;
-                                        heartbeat.heartbeat=0;
-                                        while(ros::ok()){
-                                          heartbeat_pub_->Publish(heartbeat);
-                                          std::this_thread::sleep_for(std::chrono::milliseconds(300));
-                                        }
-                                      }
+                                    roborts_sdk::cmd_heartbeat heartbeat;
+                                    heartbeat.heartbeat=0;
+                                    while(ros::ok()){
+                                      heartbeat_pub_->Publish(heartbeat);
+                                      std::this_thread::sleep_for(std::chrono::milliseconds(300));
+                                    }
+                                  }
   );
 
 

diff --git a/roborts_base/gimbal/gimbal.cpp b/roborts_base/gimbal/gimbal.cpp
@@ -42,10 +42,11 @@ void Gimbal::SDK_Init(){
   verison_client_->AsyncSendRequest(version,
                                     [](roborts_sdk::Client<roborts_sdk::cmd_version_id,
                                                            roborts_sdk::cmd_version_id>::SharedFuture future) {
-                                      LOG_INFO << "Gimbal Firmware Version: " << int(future.get()->version_id>>24&0xFF) <<"."
-                                               <<int(future.get()->version_id>>16&0xFF)<<"."
-                                               <<int(future.get()->version_id>>8&0xFF)<<"."
-                                               <<int(future.get()->version_id&0xFF);
+                                      ROS_INFO("Gimbal Firmware Version: %d.%d.%d.%d",
+                                               int(future.get()->version_id>>24&0xFF),
+                                               int(future.get()->version_id>>16&0xFF),
+                                               int(future.get()->version_id>>8&0xFF),
+                                               int(future.get()->version_id&0xFF));
                                     });
 
   handle_->CreateSubscriber<roborts_sdk::cmd_gimbal_info>(GIMBAL_CMD_SET, CMD_PUSH_GIMBAL_INFO,

diff --git a/roborts_base/roborts_sdk/dispatch/execution.cpp b/roborts_base/roborts_sdk/dispatch/execution.cpp
@@ -23,7 +23,7 @@ std::shared_ptr<Handle> Executor::GetHandle() {
   return handle_;
 }
 
-void Executor::ExecuteSubscription(std::shared_ptr<SubscriptionBase> subscription) {
+void Executor::ExecuteSubscription(const std::shared_ptr<SubscriptionBase>& subscription) {
   auto message_header = subscription->CreateMessageHeader();
   std::shared_ptr<void> message = subscription->CreateMessage();
 
@@ -39,7 +39,7 @@ void Executor::ExecuteSubscription(std::shared_ptr<SubscriptionBase> subscriptio
   //TODO: add return message;
   //subscription->return_message(message);
 }
-void Executor::ExecuteService(std::shared_ptr<ServiceBase> service) {
+void Executor::ExecuteService(const std::shared_ptr<ServiceBase>& service) {
   auto request_header = service->CreateRequestHeader();
   std::shared_ptr<void> request = service->CreateRequest();
 
@@ -53,7 +53,7 @@ void Executor::ExecuteService(std::shared_ptr<ServiceBase> service) {
     DLOG_ERROR << "take request failed!";
   }
 }
-void Executor::ExecuteClient(std::shared_ptr<ClientBase> client) {
+void Executor::ExecuteClient(const std::shared_ptr<ClientBase>& client) {
   auto request_header = client->CreateRequestHeader();
   std::shared_ptr<void> response = client->CreateResponse();
 

diff --git a/roborts_base/roborts_sdk/dispatch/execution.h b/roborts_base/roborts_sdk/dispatch/execution.h
@@ -46,17 +46,17 @@ class Executor {
    * @brief Given the subscription, invoke its callback function
    * @param subscription Subscription base pointer of certain command
    */
-  void ExecuteSubscription(std::shared_ptr<SubscriptionBase> subscription);
+  void ExecuteSubscription(const std::shared_ptr<SubscriptionBase>& subscription);
   /**
    * @brief Given the service, invoke its callback function and call the protocol layer to send response/ack
    * @param service Service base pointer of certain command
    */
-  void ExecuteService(std::shared_ptr<ServiceBase> service);
+  void ExecuteService(const std::shared_ptr<ServiceBase>& service);
   /**
    * @brief Given the client, call the protocol layer to send command and wait for the response/ack
    * @param client Client base pointer of certain command
    */
-  void ExecuteClient(std::shared_ptr<ClientBase> client);
+  void ExecuteClient(const std::shared_ptr<ClientBase>& client);
 
  private:
   //! pointer of handle

diff --git a/roborts_base/roborts_sdk/dispatch/handle.cpp b/roborts_base/roborts_sdk/dispatch/handle.cpp
@@ -39,7 +39,7 @@ bool Handle::Init(){
   LOG_INFO<<"Initialization of protocol layer and dispatch layer succeeded. ";
   return true;
 }
-std::shared_ptr<Protocol> Handle::GetProtocol() {
+std::shared_ptr<Protocol>& Handle::GetProtocol() {
   return protocol_;
 }
 

diff --git a/roborts_base/roborts_sdk/dispatch/handle.h b/roborts_base/roborts_sdk/dispatch/handle.h
@@ -68,7 +68,7 @@ class Handle : public std::enable_shared_from_this<Handle> {
    * @brief Get the pointer of protocol layer
    * @return The pointer of protocol layer
    */
-  std::shared_ptr<Protocol> GetProtocol();
+  std::shared_ptr<Protocol>& GetProtocol();
   /**
    * @brief Create the subscriber for the protocol command without need of ack (Receive command)
    * @tparam Cmd Command DataType

diff --git a/roborts_base/roborts_sdk/hardware/serial_device.cpp b/roborts_base/roborts_sdk/hardware/serial_device.cpp
@@ -162,7 +162,7 @@ int SerialDevice::Read(uint8_t *buf, int len) {
     return -1;
   } else {
     ret = read(serial_fd_, buf, len);
-    //LOG_INFO<<"Read once length:"<<ret;
+    DLOG_INFO<<"Read once length: "<<ret;
     while (ret == 0) {
       LOG_ERROR << "Connection closed, try to reconnect.";
       while (!Init()) {

diff --git a/roborts_base/roborts_sdk/protocol/protocol.cpp b/roborts_base/roborts_sdk/protocol/protocol.cpp
@@ -54,8 +54,8 @@ bool Protocol::Init() {
   auto max_buffer_size = BUFFER_SIZE;
   auto max_pack_size = MAX_PACK_SIZE;
   auto session_table_num = SESSION_TABLE_NUM;
-  memory_pool_ptr_ = std::make_shared<MemoryPool>(max_buffer_size,
-                                                  max_pack_size,
+  memory_pool_ptr_ = std::make_shared<MemoryPool>(max_pack_size,
+                                                  max_buffer_size,
                                                   session_table_num);
   memory_pool_ptr_->Init();
 
@@ -96,11 +96,11 @@ void Protocol::AutoRepeatSendCheck() {
 
             if (cmd_session_table_[i].sent >= cmd_session_table_[i].retry_time) {
               LOG_ERROR << "Sending timeout, Free session "
-                         << static_cast<int>(cmd_session_table_[i].session_id);
+                        << static_cast<int>(cmd_session_table_[i].session_id);
               FreeCMDSession(&cmd_session_table_[i]);
             } else {
               LOG_ERROR << "Retry session "
-                         << static_cast<int>(cmd_session_table_[i].session_id);
+                        << static_cast<int>(cmd_session_table_[i].session_id);
               DeviceSend(cmd_session_table_[i].memory_block_ptr->memory_ptr);
               cmd_session_table_[i].pre_time_stamp = current_time_stamp;
               cmd_session_table_[i].sent++;
@@ -121,8 +121,11 @@ void Protocol::AutoRepeatSendCheck() {
 }
 
 void Protocol::ReceivePool() {
+  std::chrono::steady_clock::time_point start_time, end_time;
+  std::chrono::microseconds execution_duration;
+  std::chrono::microseconds cycle_duration = std::chrono::microseconds(int(1e6/READING_RATE));
   while (running_) {
-
+    start_time = std::chrono::steady_clock::now();
     RecvContainer *container_ptr = Receive();
     if (container_ptr) {
       if (buffer_pool_map_.count(std::make_pair(container_ptr->command_info.cmd_set,
@@ -132,17 +135,23 @@ void Protocol::ReceivePool() {
             = std::make_shared<CircularBuffer<RecvContainer>>(100);
 
         DLOG_INFO<<"Capture command: "
-                <<"cmd set: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.cmd_set)
-                <<", cmd id: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.cmd_id)
-                <<", sender: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.sender)
-                <<", receiver: 0x" <<std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.receiver);
+                 <<"cmd set: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.cmd_set)
+                 <<", cmd id: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.cmd_id)
+                 <<", sender: 0x"<< std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.sender)
+                 <<", receiver: 0x" <<std::setw(2) << std::hex << std::setfill('0') << int(container_ptr->command_info.receiver);
 
       }
       //1 time copy
       buffer_pool_map_[std::make_pair(container_ptr->command_info.cmd_set,
                                       container_ptr->command_info.cmd_id)]->Push(*container_ptr);
     }
-    usleep(100);
+    end_time = std::chrono::steady_clock::now();
+    std::chrono::microseconds execution_duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time);
+    if (cycle_duration > execution_duration){
+      std::this_thread::sleep_for(cycle_duration - execution_duration);
+    }
+
   }
 }
 
@@ -166,39 +175,39 @@ bool Protocol::Take(const CommandInfo *command_info,
 
     if (int(container.command_info.need_ack) != int(command_info->need_ack)){
       DLOG_ERROR << "Requested need_ack: "<< int(command_info->need_ack)
-                << ", Get need_ack: "<< int(container.command_info.need_ack);
+                 << ", Get need_ack: "<< int(container.command_info.need_ack);
       mismatch = true;
     }
 
     if (container.message_header.is_ack){
       if (int(container.command_info.receiver) != int(command_info->sender)){
         DLOG_ERROR << "Requested ACK receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(command_info->sender)
-                  << ", Get ACK receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.receiver);
+                   << ", Get ACK receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.receiver);
         mismatch = true;
       }
       if (int(container.command_info.sender) != int(command_info->receiver)){
         DLOG_ERROR << "Requested ACK sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(command_info->receiver)
-                  << ", Get ACK sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.sender);
+                   << ", Get ACK sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.sender);
         mismatch = true;
       }
     }
     else{
       if (int(container.command_info.receiver) != int(command_info->receiver)){
         DLOG_ERROR << "Requested receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.receiver)
-                  << ", Get receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.receiver);
+                   << ", Get receiver: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.receiver);
         mismatch = true;
       }
 
       if (int(container.command_info.sender) != int(command_info->sender)){
         DLOG_ERROR << "Requested sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(command_info->sender)
-                  << ", Get sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.sender);
+                   << ", Get sender: "<< std::setw(2) << std::hex << std::setfill('0') << int(container.command_info.sender);
         mismatch = true;
       }
     }
 
     if (int(container.command_info.length) !=int(command_info->length)){
       DLOG_ERROR << "Requested length: "<< int(command_info->length)
-                <<", Get length: "<< int(container.command_info.length);
+                 <<", Get length: "<< int(container.command_info.length);
       mismatch = true;
     }
     if(mismatch){
@@ -601,7 +610,7 @@ bool Protocol::DeviceSend(uint8_t *buf) {
   } else if (ans != header_ptr->length) {
     DLOG_ERROR << "Port send failed, send length:" << ans << "package length" << header_ptr->length;
   } else {
-    DLOG_INFO << "Port send success.";
+    DLOG_INFO << "Port send success with length: " << header_ptr->length;
     return true;
   }
   return false;
@@ -655,6 +664,7 @@ bool Protocol::ByteHandler(const uint8_t byte) {
 
   if (reuse_buffer_) {
     if (recv_stream_ptr_->reuse_count != 0) {
+
       while (recv_stream_ptr_->reuse_index < MAX_PACK_SIZE) {
         /*! @note because reuse_index maybe re-located, so reuse_index must
          *  be
@@ -708,13 +718,14 @@ bool Protocol::VerifyHeader() {
       (header_ptr->length < MAX_PACK_SIZE) && (header_ptr->reserved0 == 0) &&
       (header_ptr->reserved1 == 0) && (header_ptr->receiver == DEVICE || header_ptr->receiver == 0xFF) &&
       CRCHeadCheck((uint8_t *) header_ptr, HEADER_LEN)) {
-    // It is an unused part because minimum package is more longer than a header
+    // It is an unused part because minimum package is at least longer than a header
     if (header_ptr->length == HEADER_LEN) {
 
       is_frame = ContainerHandler();
       //prepare data stream
       PrepareStream();
     }
+
   } else {
     //shift the data stream
     ShiftStream();
@@ -918,7 +929,7 @@ void Protocol::ReuseStream() {
   recv_stream_ptr_->recv_index = HEADER_LEN;
   ShiftStream();
 
-  recv_stream_ptr_->recv_index = n_dest_index;
+  recv_stream_ptr_->reuse_index = n_dest_index;
   recv_stream_ptr_->reuse_count++;
 }
 

diff --git a/roborts_base/roborts_sdk/protocol/protocol.h b/roborts_base/roborts_sdk/protocol/protocol.h
@@ -446,10 +446,12 @@ class Protocol {
   bool CRCTailCheck(uint8_t *data_ptr, size_t length);
   /******************* Const List ***************************/
 
+  //! rate of buffer reading
+  static const int    READING_RATE = 8000;
   //! size of receive buffer used to read from hardware device
-  static const size_t BUFFER_SIZE = 1024;
+  static const size_t BUFFER_SIZE = 4096;
   //! max Size of package
-  static const size_t MAX_PACK_SIZE = 1024;
+  static const size_t MAX_PACK_SIZE = 4096;
   //! session number for a sender/receiver
   static const size_t SESSION_TABLE_NUM = 32;
   //! length of header

diff --git a/roborts_bringup/launch/base.launch b/roborts_bringup/launch/base.launch
@@ -3,7 +3,7 @@
   <rosparam command="load" file="$(find roborts_base)/config/roborts_base_parameter.yaml" />
 
   <!-- Run the Base Node -->
-  <node pkg="roborts_base" type="roborts_base_node" name="roborts_base_node" respawn="false" />
+  <node pkg="roborts_base" type="roborts_base_node" name="roborts_base_node" output="screen" respawn="true" />
 
 </launch>