Merge pull request #38 from ROBOTIS-GIT/master
Merge for sync kinetic-devel and master branch
This commit is contained in:
@ -95,8 +95,6 @@ private:
|
||||
void initializeSyncWrite();
|
||||
|
||||
public:
|
||||
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
|
||||
|
||||
bool DEBUG_PRINT;
|
||||
Robot *robot_;
|
||||
|
||||
|
@ -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,7 +72,7 @@ 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);
|
||||
@ -80,7 +80,7 @@ void RobotisController::initializeSyncWrite()
|
||||
} 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)
|
||||
|
30
robotis_device/devices/sensor/OPEN-CR.device
Normal file
30
robotis_device/devices/sensor/OPEN-CR.device
Normal file
@ -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
|
||||
|
||||
|
@ -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();
|
||||
};
|
||||
|
||||
}
|
||||
|
@ -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_;
|
||||
}
|
||||
|
Reference in New Issue
Block a user