Merge pull request #36 from ROBOTIS-GIT/develop

Develop

robotis_controller/include/robotis_controller/robotis_controller.h

@@ -95,14 +95,11 @@ private:
   void initializeSyncWrite();
 
 public:
-  static const int  DEFAULT_CONTROL_CYCLE_MSEC  = 8;    // 8 msec
-
   bool              DEBUG_PRINT;
   Robot            *robot_;
 
   bool              gazebo_mode_;
   std::string       gazebo_robot_name_;
-  int               control_cycle_msec_;
 
   /* bulk read */
   std::map<std::string, dynamixel::GroupBulkRead *>   port_to_bulk_read_;

robotis_controller/src/robotis_controller/robotis_controller.cpp

@@ -51,8 +51,7 @@ RobotisController::RobotisController()
     DEBUG_PRINT(false),
     robot_(0),
     gazebo_mode_(false),
-    gazebo_robot_name_("robotis"),
-    control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC)
+    gazebo_robot_name_("robotis")
 {
   direct_sync_write_.clear();
 }
@@ -604,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path)
 
 void RobotisController::gazeboTimerThread()
 {
-  ros::Rate gazebo_rate(1000 / control_cycle_msec_);
+  ros::Rate gazebo_rate(1000 / robot_->getControlCycle());
 
   while (!stop_timer_)
   {
@@ -663,7 +662,7 @@ void RobotisController::msgQueueThread()
   ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules",
                                                         &RobotisController::getCtrlModuleCallback, this);
 
-  ros::WallDuration duration(control_cycle_msec_ / 1000.0);
+  ros::WallDuration duration(robot_->getControlCycle() / 1000.0);
   while(ros_node.ok())
     callback_queue.callAvailable(duration);
 }
@@ -680,8 +679,8 @@ void *RobotisController::timerThread(void *param)
 
   while (!controller->stop_timer_)
   {
-    next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000;
-    next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000;
+    next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000;
+    next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000;
 
     controller->process();
 
@@ -1430,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module)
     }
   }
 
-  module->initialize(control_cycle_msec_, robot_);
+  module->initialize(robot_->getControlCycle(), robot_);
   motion_modules_.push_back(module);
   motion_modules_.unique();
 }
@@ -1452,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module)
     }
   }
 
-  module->initialize(control_cycle_msec_, robot_);
+  module->initialize(robot_->getControlCycle(), robot_);
   sensor_modules_.push_back(module);
   sensor_modules_.unique();
 }
@@ -1732,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::
   for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
   {
     while((*_stop_m_it)->isRunning())
-      usleep(control_cycle_msec_ * 1000);
+      usleep(robot_->getControlCycle() * 1000);
   }
 
   // disable module(s)
@@ -1947,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
   for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)
   {
     while ((*stop_m_it)->isRunning())
-      usleep(control_cycle_msec_ * 1000);
+      usleep(robot_->getControlCycle() * 1000);
   }
 
   // disable module(s)

robotis_device/include/robotis_device/robot.h

@@ -48,17 +48,23 @@
 #define DYNAMIXEL             "dynamixel"
 #define SENSOR                "sensor"
 
+#define SESSION_CONTROL_INFO  "control info"
 #define SESSION_PORT_INFO     "port info"
 #define SESSION_DEVICE_INFO   "device info"
 
 #define SESSION_TYPE_INFO     "type info"
 #define SESSION_CONTROL_TABLE "control table"
 
+#define DEFAULT_CONTROL_CYCLE 8 // milliseconds
+
 namespace robotis_framework
 {
 
 class Robot
 {
+private:
+  int   control_cycle_msec_;
+
 public:
   std::map<std::string, dynamixel::PortHandler *> ports_;   // string: port name
   std::map<std::string, std::string>  port_default_device_; // port name, default device name
@@ -70,6 +76,8 @@ public:
 
   Sensor     *getSensor(std::string path, int id, std::string port, float protocol_version);
   Dynamixel  *getDynamixel(std::string path, int id, std::string port, float protocol_version);
+
+  int         getControlCycle();
 };
 
 }

robotis_device/src/robotis_device/robot.cpp

@@ -76,6 +76,7 @@ static inline std::vector<std::string> split(const std::string &text, char sep)
 }
 
 Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
+  : control_cycle_msec_(DEFAULT_CONTROL_CYCLE)
 {
   if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)
     dev_desc_dir_path += "/";
@@ -106,7 +107,16 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
         continue;
       }
 
-      if (session == SESSION_PORT_INFO)
+      if (session == SESSION_CONTROL_INFO)
+      {
+        std::vector<std::string> tokens = split(input_str, '=');
+        if (tokens.size() != 2)
+          continue;
+
+        if (tokens[0] == "control_cycle")
+          control_cycle_msec_ = std::atoi(tokens[1].c_str());
+      }
+      else if (session == SESSION_PORT_INFO)
       {
         std::vector<std::string> tokens = split(input_str, '|');
         if (tokens.size() != 3)
@@ -485,3 +495,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
   return dxl;
 }
 
+int Robot::getControlCycle()
+{
+  return control_cycle_msec_;
+}