BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

read control cycle from .robot file

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-12-14 17:19:52 +09:00
parent 0afd62afb3
commit 41a8e30731
4 changed files with 32 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
robotis_controller/include/robotis_controller/robotis_controller.h
View File

@ -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_;

19
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -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)

8
robotis_device/include/robotis_device/robot.h
View File

@ -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();
};
}

16
robotis_device/src/robotis_device/robot.cpp
View File

@ -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_;
}