Skip to content

Commit 3f64c07

Browse files
Merge pull request #36 from ROBOTIS-GIT/develop
Develop
2 parents 891cf2f + 8660bd0 commit 3f64c07

File tree

4 files changed

+32
-14
lines changed

4 files changed

+32
-14
lines changed

robotis_controller/include/robotis_controller/robotis_controller.h

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -95,14 +95,11 @@ class RobotisController : public Singleton<RobotisController>
9595
void initializeSyncWrite();
9696

9797
public:
98-
static const int DEFAULT_CONTROL_CYCLE_MSEC = 8; // 8 msec
99-
10098
bool DEBUG_PRINT;
10199
Robot *robot_;
102100

103101
bool gazebo_mode_;
104102
std::string gazebo_robot_name_;
105-
int control_cycle_msec_;
106103

107104
/* bulk read */
108105
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;

robotis_controller/src/robotis_controller/robotis_controller.cpp

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -51,8 +51,7 @@ RobotisController::RobotisController()
5151
DEBUG_PRINT(false),
5252
robot_(0),
5353
gazebo_mode_(false),
54-
gazebo_robot_name_("robotis"),
55-
control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC)
54+
gazebo_robot_name_("robotis")
5655
{
5756
direct_sync_write_.clear();
5857
}
@@ -604,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path)
604603

605604
void RobotisController::gazeboTimerThread()
606605
{
607-
ros::Rate gazebo_rate(1000 / control_cycle_msec_);
606+
ros::Rate gazebo_rate(1000 / robot_->getControlCycle());
608607

609608
while (!stop_timer_)
610609
{
@@ -663,7 +662,7 @@ void RobotisController::msgQueueThread()
663662
ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules",
664663
&RobotisController::getCtrlModuleCallback, this);
665664

666-
ros::WallDuration duration(control_cycle_msec_ / 1000.0);
665+
ros::WallDuration duration(robot_->getControlCycle() / 1000.0);
667666
while(ros_node.ok())
668667
callback_queue.callAvailable(duration);
669668
}
@@ -680,8 +679,8 @@ void *RobotisController::timerThread(void *param)
680679

681680
while (!controller->stop_timer_)
682681
{
683-
next_time.tv_sec += (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) / 1000000000;
684-
next_time.tv_nsec = (next_time.tv_nsec + controller->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;
685684

686685
controller->process();
687686

@@ -1430,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module)
14301429
}
14311430
}
14321431

1433-
module->initialize(control_cycle_msec_, robot_);
1432+
module->initialize(robot_->getControlCycle(), robot_);
14341433
motion_modules_.push_back(module);
14351434
motion_modules_.unique();
14361435
}
@@ -1452,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module)
14521451
}
14531452
}
14541453

1455-
module->initialize(control_cycle_msec_, robot_);
1454+
module->initialize(robot_->getControlCycle(), robot_);
14561455
sensor_modules_.push_back(module);
14571456
sensor_modules_.unique();
14581457
}
@@ -1732,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::
17321731
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
17331732
{
17341733
while((*_stop_m_it)->isRunning())
1735-
usleep(control_cycle_msec_ * 1000);
1734+
usleep(robot_->getControlCycle() * 1000);
17361735
}
17371736

17381737
// disable module(s)
@@ -1947,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
19471946
for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)
19481947
{
19491948
while ((*stop_m_it)->isRunning())
1950-
usleep(control_cycle_msec_ * 1000);
1949+
usleep(robot_->getControlCycle() * 1000);
19511950
}
19521951

19531952
// disable module(s)

robotis_device/include/robotis_device/robot.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,17 +48,23 @@
4848
#define DYNAMIXEL "dynamixel"
4949
#define SENSOR "sensor"
5050

51+
#define SESSION_CONTROL_INFO "control info"
5152
#define SESSION_PORT_INFO "port info"
5253
#define SESSION_DEVICE_INFO "device info"
5354

5455
#define SESSION_TYPE_INFO "type info"
5556
#define SESSION_CONTROL_TABLE "control table"
5657

58+
#define DEFAULT_CONTROL_CYCLE 8 // milliseconds
59+
5760
namespace robotis_framework
5861
{
5962

6063
class Robot
6164
{
65+
private:
66+
int control_cycle_msec_;
67+
6268
public:
6369
std::map<std::string, dynamixel::PortHandler *> ports_; // string: port name
6470
std::map<std::string, std::string> port_default_device_; // port name, default device name
@@ -70,6 +76,8 @@ class Robot
7076

7177
Sensor *getSensor(std::string path, int id, std::string port, float protocol_version);
7278
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
79+
80+
int getControlCycle();
7381
};
7482

7583
}

robotis_device/src/robotis_device/robot.cpp

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,7 @@ static inline std::vector<std::string> split(const std::string &text, char sep)
7676
}
7777

7878
Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
79+
: control_cycle_msec_(DEFAULT_CONTROL_CYCLE)
7980
{
8081
if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1, "/") != 0)
8182
dev_desc_dir_path += "/";
@@ -106,7 +107,16 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
106107
continue;
107108
}
108109

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)
110120
{
111121
std::vector<std::string> tokens = split(input_str, '|');
112122
if (tokens.size() != 3)
@@ -485,3 +495,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
485495
return dxl;
486496
}
487497

498+
int Robot::getControlCycle()
499+
{
500+
return control_cycle_msec_;
501+
}

0 commit comments

Comments
 (0)