- Direct Control Mode bug fixed.

Author: ROBOTIS-zerom

robotis_controller/src/robotis_controller/robotis_controller.cpp

@@ -1109,14 +1109,64 @@ void RobotisController::process()
   {
     if(gazebo_mode_ == false)
     {
-      queue_mutex_.lock();
+      // BulkRead Rx
+      for (auto& it : port_to_bulk_read_)
+      {
+        robot_->ports_[it.first]->setPacketTimeout(0.0);
+        it.second->rxPacket();
+      }
 
-      for (auto& it : port_to_sync_write_position_)
+      // -> save to robot->dxls_[]->dxl_state_
+      if (robot_->dxls_.size() > 0)
       {
-        it.second->txPacket();
-        it.second->clearParam();
+        for (auto& dxl_it : robot_->dxls_)
+        {
+          Dynamixel  *dxl         = dxl_it.second;
+          std::string port_name   = dxl_it.second->port_name_;
+          std::string joint_name  = dxl_it.first;
+
+          if (dxl->bulk_read_items_.size() > 0)
+          {
+            uint32_t data = 0;
+            for (int i = 0; i < dxl->bulk_read_items_.size(); i++)
+            {
+              ControlTableItem *item = dxl->bulk_read_items_[i];
+              if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true)
+              {
+                data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_);
+
+                // change dxl_state
+                if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_)
+                  dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_)
+                  dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data);
+                else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_)
+                  dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data);
+                else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_)
+                  dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_)
+                  dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
+                else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
+                  dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data);
+                else
+                  dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
+              }
+            }
+
+            // -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
+            dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
+          }
+        }
       }
 
+      queue_mutex_.lock();
+
+//      for (auto& it : port_to_sync_write_position_)
+//      {
+//        it.second->txPacket();
+//        it.second->clearParam();
+//      }
+
       if (direct_sync_write_.size() > 0)
       {
         for (int i = 0; i < direct_sync_write_.size(); i++)
@@ -1128,6 +1178,10 @@ void RobotisController::process()
       }
 
       queue_mutex_.unlock();
+
+      // BulkRead Tx
+      for (auto& it : port_to_bulk_read_)
+        it.second->txPacket();
     }
   }
 
@@ -1553,9 +1607,22 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
 void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg)
 {
   if (msg->data == "DirectControlMode")
+  {
+    for (auto& it : port_to_bulk_read_)
+    {
+      robot_->ports_[it.first]->setPacketTimeout(0.0);
+      it.second->rxPacket();
+    }
     controller_mode_ = DirectControlMode;
+  }
   else if (msg->data == "MotionModuleMode")
+  {
+    for (auto& it : port_to_bulk_read_)
+    {
+      it.second->txPacket();
+    }
     controller_mode_ = MotionModuleMode;
+  }
 }
 
 void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)