Now, control cycle can be changed.
This commit is contained in:
@ -95,13 +95,14 @@ private:
|
||||
void initializeSyncWrite();
|
||||
|
||||
public:
|
||||
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
|
||||
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_;
|
||||
|
@ -51,7 +51,8 @@ RobotisController::RobotisController()
|
||||
DEBUG_PRINT(false),
|
||||
robot_(0),
|
||||
gazebo_mode_(false),
|
||||
gazebo_robot_name_("robotis")
|
||||
gazebo_robot_name_("robotis"),
|
||||
control_cycle_msec_(DEFAULT_CONTROL_CYCLE_MSEC)
|
||||
{
|
||||
direct_sync_write_.clear();
|
||||
}
|
||||
@ -603,7 +604,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 / control_cycle_msec_);
|
||||
|
||||
while (!stop_timer_)
|
||||
{
|
||||
@ -662,7 +663,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(control_cycle_msec_ / 1000.0);
|
||||
while(ros_node.ok())
|
||||
callback_queue.callAvailable(duration);
|
||||
}
|
||||
@ -679,8 +680,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->control_cycle_msec_ * 1000000) / 1000000000;
|
||||
next_time.tv_nsec = (next_time.tv_nsec + controller->control_cycle_msec_ * 1000000) % 1000000000;
|
||||
|
||||
controller->process();
|
||||
|
||||
@ -1429,7 +1430,7 @@ void RobotisController::addMotionModule(MotionModule *module)
|
||||
}
|
||||
}
|
||||
|
||||
module->initialize(CONTROL_CYCLE_MSEC, robot_);
|
||||
module->initialize(control_cycle_msec_, robot_);
|
||||
motion_modules_.push_back(module);
|
||||
motion_modules_.unique();
|
||||
}
|
||||
@ -1451,7 +1452,7 @@ void RobotisController::addSensorModule(SensorModule *module)
|
||||
}
|
||||
}
|
||||
|
||||
module->initialize(CONTROL_CYCLE_MSEC, robot_);
|
||||
module->initialize(control_cycle_msec_, robot_);
|
||||
sensor_modules_.push_back(module);
|
||||
sensor_modules_.unique();
|
||||
}
|
||||
@ -1731,7 +1732,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(control_cycle_msec_ * 1000);
|
||||
}
|
||||
|
||||
// disable module(s)
|
||||
@ -1946,7 +1947,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(control_cycle_msec_ * 1000);
|
||||
}
|
||||
|
||||
// disable module(s)
|
||||
|
Reference in New Issue
Block a user