Merge pull request #38 from ROBOTIS-GIT/master

robotpilot · web-flow · commit 69b4b5fc8bc2 · 2017-04-24T17:30:27.000+09:00
Merge for sync kinetic-devel and master branch
diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h
@@ -95,8 +95,6 @@ class RobotisController : public Singleton<RobotisController>
   void initializeSyncWrite();
 
 public:
-  static const int  CONTROL_CYCLE_MSEC  = 8;    // 8 msec
-
   bool              DEBUG_PRINT;
   Robot            *robot_;
 
diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp
@@ -61,7 +61,7 @@ void RobotisController::initializeSyncWrite()
   if (gazebo_mode_ == true)
     return;
 
-  ROS_INFO("FIRST BULKREAD");
+  //ROS_INFO("FIRST BULKREAD");
   for (auto& it : port_to_bulk_read_)
     it.second->txRxPacket();
   for(auto& it : port_to_bulk_read_)
@@ -72,15 +72,15 @@ void RobotisController::initializeSyncWrite()
     {
       if (++error_count > 10)
       {
-        ROS_ERROR("[RobotisController] bulk read fail!!");
+        ROS_ERROR("[RobotisController] first bulk read fail!!");
         exit(-1);
       }
       usleep(10 * 1000);
       result = it.second->txRxPacket();
     } while (result != COMM_SUCCESS);
   }
   init_pose_loaded_ = true;
-  ROS_INFO("FIRST BULKREAD END");
+  //ROS_INFO("FIRST BULKREAD END");
 
   // clear syncwrite param setting
   for (auto& it : port_to_sync_write_position_)
@@ -603,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_)
   {
@@ -662,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);
 }
@@ -679,8 +679,8 @@ void *RobotisController::timerThread(void *param)
 
   while (!controller->stop_timer_)
   {
-    next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000;
-    next_time.tv_nsec = (next_time.tv_nsec + 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();
 
@@ -1429,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();
 }
@@ -1451,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();
 }
@@ -1731,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)
@@ -1946,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)
diff --git a/robotis_device/devices/sensor/OPEN-CR.device b/robotis_device/devices/sensor/OPEN-CR.device
@@ -0,0 +1,30 @@
+[device info]
+model_name  = OPEN-CR
+device_type = sensor
+
+[control table]
+# addr | item name                | length | access | memory |   min value   |  max value  | signed
+   0   | model_number             | 2      | R      | EEPROM | 0             | 65535       | N
+   2   | version_of_firmware      | 1      | R      | EEPROM | 0             | 254         | N
+   3   | ID                       | 1      | RW     | EEPROM | 0             | 252         | N
+   4   | baudrate                 | 1      | RW     | EEPROM | 0             | 252         | N
+   5   | return_delay_time        | 1      | RW     | EEPROM | 0             | 254         | N
+   16  | status_return_level      | 1      | RW     | EEPROM | 0             | 2           | N
+   24  | torque_enable            | 1      | RW     | RAM    | 0             | 1           | N
+   25  | LED                      | 1      | RW     | RAM    | 0             | 7           | N
+   26  | LED_RGB                  | 2      | RW     | RAM    | 0             | 32767       | N
+   28  | buzzer                   | 2      | RW     | RAM    | 0             | 65535       | N
+   30  | button                   | 1      | R      | RAM    | 0             | 15          | N
+   31  | present_voltage          | 1      | R      | RAM    | 50            | 250         | N
+   32  | gyro_x                   | 2      | R      | RAM    | -32800        | 32800       | Y
+   34  | gyro_y                   | 2      | R      | RAM    | -32800        | 32800       | Y
+   36  | gyro_z                   | 2      | R      | RAM    | -32800        | 32800       | Y
+   38  | acc_x                    | 2      | R      | RAM    | -32768        | 32768       | Y
+   40  | acc_y                    | 2      | R      | RAM    | -32768        | 32768       | Y
+   42  | acc_z                    | 2      | R      | RAM    | -32768        | 32768       | Y
+   44  | roll                     | 2      | R      | RAM    | 0             | 4096        | N
+   46  | pitch                    | 2      | R      | RAM    | 0             | 4096        | N
+   48  | yaw                      | 2      | R      | RAM    | 0             | 4096        | N
+   50  | imu_control              | 1      | RW     | RAM    | 0             | 255         | N
+   
+   
diff --git a/robotis_device/include/robotis_device/robot.h b/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 @@ class Robot
 
   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();
 };
 
 }
diff --git a/robotis_device/src/robotis_device/robot.cpp b/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_;
+}

Original file line number	Diff line number	Diff line change
`@@ -61,7 +61,7 @@ void RobotisController::initializeSyncWrite()`
`61`	`61`	`if (gazebo_mode_ == true)`
`62`	`62`	`return;`
`63`	`63`
`64`		`- ROS_INFO("FIRST BULKREAD");`
	`64`	`+ //ROS_INFO("FIRST BULKREAD");`
`65`	`65`	`for (auto& it : port_to_bulk_read_)`
`66`	`66`	`it.second->txRxPacket();`
`67`	`67`	`for(auto& it : port_to_bulk_read_)`
`@@ -72,15 +72,15 @@ void RobotisController::initializeSyncWrite()`
`72`	`72`	`{`
`73`	`73`	`if (++error_count > 10)`
`74`	`74`	`{`
`75`		`- ROS_ERROR("[RobotisController] bulk read fail!!");`
	`75`	`+ ROS_ERROR("[RobotisController] first bulk read fail!!");`
`76`	`76`	`exit(-1);`
`77`	`77`	`}`
`78`	`78`	`usleep(10 * 1000);`
`79`	`79`	`result = it.second->txRxPacket();`
`80`	`80`	`} while (result != COMM_SUCCESS);`
`81`	`81`	`}`
`82`	`82`	`init_pose_loaded_ = true;`
`83`		`- ROS_INFO("FIRST BULKREAD END");`
	`83`	`+ //ROS_INFO("FIRST BULKREAD END");`
`84`	`84`
`85`	`85`	`// clear syncwrite param setting`
`86`	`86`	`for (auto& it : port_to_sync_write_position_)`
`@@ -603,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path)`
`603`	`603`
`604`	`604`	`void RobotisController::gazeboTimerThread()`
`605`	`605`	`{`
`606`		`- ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC);`
	`606`	`+ ros::Rate gazebo_rate(1000 / robot_->getControlCycle());`
`607`	`607`
`608`	`608`	`while (!stop_timer_)`
`609`	`609`	`{`
`@@ -662,7 +662,7 @@ void RobotisController::msgQueueThread()`
`662`	`662`	`ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules",`
`663`	`663`	`&RobotisController::getCtrlModuleCallback, this);`
`664`	`664`
`665`		`- ros::WallDuration duration(CONTROL_CYCLE_MSEC / 1000.0);`
	`665`	`+ ros::WallDuration duration(robot_->getControlCycle() / 1000.0);`
`666`	`666`	`while(ros_node.ok())`
`667`	`667`	`callback_queue.callAvailable(duration);`
`668`	`668`	`}`
`@@ -679,8 +679,8 @@ void RobotisController::timerThread(void param)`
`679`	`679`
`680`	`680`	`while (!controller->stop_timer_)`
`681`	`681`	`{`
`682`		`- next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000;`
`683`		`- next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000;`
	`682`	`+ next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000;`
	`683`	`+ next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000;`
`684`	`684`
`685`	`685`	`controller->process();`
`686`	`686`
`@@ -1429,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module)`
`1429`	`1429`	`}`
`1430`	`1430`	`}`
`1431`	`1431`
`1432`		`- module->initialize(CONTROL_CYCLE_MSEC, robot_);`
	`1432`	`+ module->initialize(robot_->getControlCycle(), robot_);`
`1433`	`1433`	`motion_modules_.push_back(module);`
`1434`	`1434`	`motion_modules_.unique();`
`1435`	`1435`	`}`
`@@ -1451,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module)`
`1451`	`1451`	`}`
`1452`	`1452`	`}`
`1453`	`1453`
`1454`		`- module->initialize(CONTROL_CYCLE_MSEC, robot_);`
	`1454`	`+ module->initialize(robot_->getControlCycle(), robot_);`
`1455`	`1455`	`sensor_modules_.push_back(module);`
`1456`	`1456`	`sensor_modules_.unique();`
`1457`	`1457`	`}`
`@@ -1731,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::`
`1731`	`1731`	`for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)`
`1732`	`1732`	`{`
`1733`	`1733`	`while((*_stop_m_it)->isRunning())`
`1734`		`- usleep(CONTROL_CYCLE_MSEC * 1000);`
	`1734`	`+ usleep(robot_->getControlCycle() * 1000);`
`1735`	`1735`	`}`
`1736`	`1736`
`1737`	`1737`	`// disable module(s)`
`@@ -1946,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)`
`1946`	`1946`	`for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)`
`1947`	`1947`	`{`
`1948`	`1948`	`while ((*stop_m_it)->isRunning())`
`1949`		`- usleep(CONTROL_CYCLE_MSEC * 1000);`
	`1949`	`+ usleep(robot_->getControlCycle() * 1000);`
`1950`	`1950`	`}`
`1951`	`1951`
`1952`	`1952`	`// disable module(s)`
Original file line number	Diff line number	Diff line change
`@@ -76,6 +76,7 @@ static inline std::vector<std::string> split(const std::string &text, char sep)`
`76`	`76`	`}`
`77`	`77`
`78`	`78`	`Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)`
	`79`	`+ : control_cycle_msec_(DEFAULT_CONTROL_CYCLE)`
`79`	`80`	`{`
`80`	`81`	`if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)`
`81`	`82`	`dev_desc_dir_path += "/";`
`@@ -106,7 +107,16 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)`
`106`	`107`	`continue;`
`107`	`108`	`}`
`108`	`109`
`109`		`- if (session == SESSION_PORT_INFO)`
	`110`	`+ if (session == SESSION_CONTROL_INFO)`
	`111`	`+ {`
	`112`	`+ std::vector<std::string> tokens = split(input_str, '=');`
	`113`	`+ if (tokens.size() != 2)`
	`114`	`+ continue;`
	`115`	`+`
	`116`	`+ if (tokens[0] == "control_cycle")`
	`117`	`+ control_cycle_msec_ = std::atoi(tokens[1].c_str());`
	`118`	`+ }`
	`119`	`+ else if (session == SESSION_PORT_INFO)`
`110`	`120`	`{`
`111`	`121`	`std::vector<std::string> tokens = split(input_str, '\|');`
`112`	`122`	`if (tokens.size() != 3)`
`@@ -485,3 +495,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float`
`485`	`495`	`return dxl;`
`486`	`496`	`}`
`487`	`497`
	`498`	`+int Robot::getControlCycle()`
	`499`	`+{`
	`500`	`+ return control_cycle_msec_;`
	`501`	`+}`