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

Merge pull request #4 from ROBOTIS-GIT/add_sensor_device

Browse Source 
Add sensor device
This commit is contained in:
ROBOTIS-zerom
2016-05-20 16:24:39 +09:00
parent c84a6ae05a 243b4dfbc9
commit 968a715d25
22 changed files with 734 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
dynamixel_sdk/src/dynamixel_sdk/GroupBulkRead.cpp
View File

@@ -90,11 +90,11 @@ void GroupBulkRead::RemoveParam(UINT8_T id)
void GroupBulkRead::ClearParam()
{
if(id_list_.size() != 0)
{
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
}
if(id_list_.size() == 0)
return;
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
id_list_.clear();
address_list_.clear();

9
dynamixel_sdk/src/dynamixel_sdk/GroupBulkWrite.cpp
View File

@@ -111,14 +111,11 @@ bool GroupBulkWrite::ChangeParam(UINT8_T id, UINT16_T start_address, UINT16_T da
}
void GroupBulkWrite::ClearParam()
{
if(ph_->GetProtocolVersion() == 1.0)
if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
if(id_list_.size() != 0)
{
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
}
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
id_list_.clear();
address_list_.clear();

9
dynamixel_sdk/src/dynamixel_sdk/GroupSyncRead.cpp
View File

@@ -72,14 +72,11 @@ void GroupSyncRead::RemoveParam(UINT8_T id)
}
void GroupSyncRead::ClearParam()
{
if(ph_->GetProtocolVersion() == 1.0)
if(ph_->GetProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
if(id_list_.size() != 0)
{
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
}
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
id_list_.clear();
data_list_.clear();

10
dynamixel_sdk/src/dynamixel_sdk/GroupSyncWrite.cpp
View File

@@ -92,11 +92,11 @@ bool GroupSyncWrite::ChangeParam(UINT8_T id, UINT8_T *data)
void GroupSyncWrite::ClearParam()
{
if(id_list_.size() != 0)
{
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
}
if(id_list_.size() == 0)
return;
for(unsigned int _i = 0; _i < id_list_.size(); _i++)
delete[] data_list_[id_list_[_i]];
id_list_.clear();
data_list_.clear();

10
robotis_controller/include/robotis_controller/RobotisController.h
View File

@@ -37,11 +37,9 @@ enum CONTROLLER_MODE
DIRECT_CONTROL_MODE
};
class RobotisController
class RobotisController : public Singleton<RobotisController>
{
private:
static RobotisController *unique_instance_;
boost::thread queue_thread_;
boost::thread gazebo_thread_;
boost::thread set_module_thread_;
@@ -59,8 +57,6 @@ private:
std::map<std::string, double> sensor_result_;
RobotisController();
void QueueThread();
void GazeboThread();
void SetCtrlModuleThread(std::string ctrl_module);
@@ -91,9 +87,11 @@ public:
std::map<std::string, ros::Publisher> gazebo_joint_pub;
static void *ThreadProc(void *param);
static RobotisController *GetInstance() { return unique_instance_; }
RobotisController();
bool Initialize(const std::string robot_file_path, const std::string init_file_path);
void InitDevice(const std::string init_file_path);
void Process();
void AddMotionModule(MotionModule *module);

430
robotis_controller/src/robotis_controller/RobotisController.cpp
View File

@@ -12,8 +12,6 @@
using namespace ROBOTIS;
RobotisController *RobotisController::unique_instance_ = new RobotisController();
RobotisController::RobotisController()
: is_timer_running_(false),
stop_timer_(false),
@@ -68,17 +66,6 @@ void RobotisController::InitSyncWrite()
std::string _joint_name = _it->first;
Dynamixel *_dxl = _it->second;
CONTROL_MODE _ctrl_mode = POSITION_CONTROL;
for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
{
if((*_m_it)->module_name == _dxl->ctrl_module_name)
{
_ctrl_mode = (*_m_it)->control_mode;
break;
}
}
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
UINT32_T _read_data = 0;
@@ -102,24 +89,17 @@ void RobotisController::InitSyncWrite()
_dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_read_data) - _dxl->dxl_state->position_offset; // remove offset
_dxl->dxl_state->goal_position = _dxl->dxl_state->present_position;
if(_ctrl_mode == POSITION_CONTROL)
port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
}
else if(_dxl->present_velocity_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_velocity_item->item_name)
{
_dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_read_data);
_dxl->dxl_state->goal_velocity = _dxl->dxl_state->present_velocity;
if(_ctrl_mode == VELOCITY_CONTROL)
port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
}
else if(_dxl->present_current_item != 0 && _dxl->bulk_read_items[_i]->item_name == _dxl->present_current_item->item_name)
{
_dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_read_data);
_dxl->dxl_state->goal_current = _dxl->dxl_state->present_current;
if(_ctrl_mode == CURRENT_CONTROL)
port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
}
}
}
@@ -139,8 +119,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std:
return true;
}
// TODO: TEMPORARY CODE !!
/* temporary code start */
for(std::map<std::string, PortHandler *>::iterator _it = robot->ports.begin(); _it != robot->ports.end(); _it++)
{
std::string _port_name = _it->first;
@@ -154,31 +132,40 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std:
exit(-1);
}
Dynamixel *_port_default_dxl = robot->dxls[robot->port_default_joint[_port_name]];
if(_port_default_dxl != NULL)
_default_pkt_handler = PacketHandler::GetPacketHandler(_port_default_dxl->protocol_version);
// get the default device info of the port
std::string _default_device_name = robot->port_default_device[_port_name];
std::map<std::string, Dynamixel*>::iterator _dxl_it = robot->dxls.find(_default_device_name);
std::map<std::string, Sensor*>::iterator _sensor_it = robot->sensors.find(_default_device_name);
if(_dxl_it != robot->dxls.end())
{
Dynamixel *_default_device = _dxl_it->second;
_default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version);
port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_default_device->goal_position_item->address,
_default_device->goal_position_item->data_length);
port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_default_device->goal_velocity_item->address,
_default_device->goal_velocity_item->data_length);
port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_default_device->goal_current_item->address,
_default_device->goal_current_item->data_length);
}
else if(_sensor_it != robot->sensors.end())
{
Sensor *_default_device = _sensor_it->second;
_default_pkt_handler = PacketHandler::GetPacketHandler(_default_device->protocol_version);
}
port_to_bulk_read[_port_name] = new GroupBulkRead(_port, _default_pkt_handler);
port_to_sync_write_position[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_port_default_dxl->goal_position_item->address,
_port_default_dxl->goal_position_item->data_length);
port_to_sync_write_velocity[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_port_default_dxl->goal_velocity_item->address,
_port_default_dxl->goal_velocity_item->data_length);
port_to_sync_write_torque[_port_name] = new GroupSyncWrite(_port,
_default_pkt_handler,
_port_default_dxl->goal_current_item->address,
_port_default_dxl->goal_current_item->data_length);
}
// TODO:
// for loop ping --> no response : return false
// (for loop) check all dxls are connected.
for(std::map<std::string, Dynamixel*>::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++)
{
std::string _joint_name = _it->first;
@@ -188,11 +175,93 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std:
{
usleep(10*1000);
if(Ping(_joint_name) != 0)
ROS_ERROR("JOINT[%s] does NOT response!!", _joint_name.c_str());
ROS_ERROR("JOINT[%s] does NOT respond!!", _joint_name.c_str());
}
}
// joint(dynamixel) initialize
InitDevice(init_file_path);
// [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current )
for(std::map<std::string, Dynamixel*>::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++)
{
std::string _joint_name = _it->first;
Dynamixel *_dxl = _it->second;
if(_dxl == NULL)
continue;
int _bulkread_start_addr = 0;
int _bulkread_data_length = 0;
// bulk read default : present position
if(_dxl->present_position_item != 0)
{
_bulkread_start_addr = _dxl->present_position_item->address;
_bulkread_data_length = _dxl->present_position_item->data_length;
}
// TODO: modifing
std::map<std::string, ControlTableItem *>::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1);
if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist
{
if(_dxl->bulk_read_items.size() != 0)
{
_bulkread_start_addr = _dxl->bulk_read_items[0]->address;
_bulkread_data_length = 0;
// set indirect address
int _indirect_addr = _indirect_addr_it->second->address;
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
int _addr_leng = _dxl->bulk_read_items[_i]->data_length;
_bulkread_data_length += _addr_leng;
for(int _l = 0; _l < _addr_leng; _l++)
{
// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l);
Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l);
_indirect_addr += 2;
}
}
}
}
else // INDIRECT_ADDRESS_1 NOT exist
{
if(_dxl->bulk_read_items.size() != 0)
{
_bulkread_start_addr = _dxl->bulk_read_items[0]->address;
_bulkread_data_length = 0;
ControlTableItem *_last_item = _dxl->bulk_read_items[0];
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
int _addr = _dxl->bulk_read_items[_i]->address;
if(_addr < _bulkread_start_addr)
_bulkread_start_addr = _addr;
else if(_last_item->address < _addr)
_last_item = _dxl->bulk_read_items[_i];
}
_bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length;
}
}
// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length);
port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length);
// Torque ON
if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS)
WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1);
}
queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this));
return true;
}
void RobotisController::InitDevice(const std::string init_file_path)
{
// device initialize
if(DEBUG_PRINT) ROS_WARN("INIT FILE LOAD");
YAML::Node _doc;
@@ -287,86 +356,6 @@ bool RobotisController::Initialize(const std::string robot_file_path, const std:
} catch(const std::exception& e) {
ROS_INFO("Dynamixel Init file not found.");
}
// [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current )
for(std::map<std::string, Dynamixel*>::iterator _it = robot->dxls.begin(); _it != robot->dxls.end(); _it++)
{
std::string _joint_name = _it->first;
Dynamixel *_dxl = _it->second;
if(_dxl == NULL)
continue;
int _bulkread_start_addr = 0;
int _bulkread_data_length = 0;
// bulk read default : present position
if(_dxl->present_position_item != 0)
{
_bulkread_start_addr = _dxl->present_position_item->address;
_bulkread_data_length = _dxl->present_position_item->data_length;
}
// TODO: modifing
std::map<std::string, ControlTableItem *>::iterator _indirect_addr_it = _dxl->ctrl_table.find(INDIRECT_ADDRESS_1);
if(_indirect_addr_it != _dxl->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist
{
if(_dxl->bulk_read_items.size() != 0)
{
_bulkread_start_addr = _dxl->bulk_read_items[0]->address;
_bulkread_data_length = 0;
// set indirect address
int _indirect_addr = _indirect_addr_it->second->address;
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
int _addr_leng = _dxl->bulk_read_items[_i]->data_length;
_bulkread_data_length += _addr_leng;
for(int _l = 0; _l < _addr_leng; _l++)
{
// ROS_WARN("[%12s] INDIR_ADDR: %d, ITEM_ADDR: %d", _joint_name.c_str(), _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l);
Write2Byte(_joint_name, _indirect_addr, _dxl->ctrl_table[_dxl->bulk_read_items[_i]->item_name]->address + _l);
_indirect_addr += 2;
}
}
}
}
else // INDIRECT_ADDRESS_1 NOT exist
{
if(_dxl->bulk_read_items.size() != 0)
{
_bulkread_start_addr = _dxl->bulk_read_items[0]->address;
_bulkread_data_length = 0;
ControlTableItem *_last_item = _dxl->bulk_read_items[0];
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
int _addr = _dxl->bulk_read_items[_i]->address;
if(_addr < _bulkread_start_addr)
_bulkread_start_addr = _addr;
else if(_last_item->address < _addr)
_last_item = _dxl->bulk_read_items[_i];
}
_bulkread_data_length = _last_item->address - _bulkread_start_addr + _last_item->data_length;
}
}
// ROS_WARN("[%12s] start_addr: %d, data_length: %d", _joint_name.c_str(), _bulkread_start_addr, _bulkread_data_length);
port_to_bulk_read[_dxl->port_name]->AddParam(_dxl->id, _bulkread_start_addr, _bulkread_data_length);
// Torque ON
if(WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1) != COMM_SUCCESS)
WriteCtrlItem(_joint_name, _dxl->torque_enable_item->item_name, 1);
}
/* temporary code end */
queue_thread_ = boost::thread(boost::bind(&RobotisController::QueueThread, this));
return true;
}
void RobotisController::GazeboThread()
@@ -596,70 +585,105 @@ void RobotisController::Process()
// if(_msec > 8) std::cout << "Process duration : " << _msec << std::endl;
// -> save to Robot->dxls[]->dynamixel_state.present_position
for(std::map<std::string, Dynamixel *>::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++)
if(robot->dxls.size() > 0)
{
UINT32_T _data = 0;
std::string _port_name = dxl_it->second->port_name;
std::string _joint_name = dxl_it->first;
Dynamixel *_dxl = dxl_it->second;
_present_state.header.stamp = ros::Time::now();
_goal_state.header.stamp = _present_state.header.stamp;
if(gazebo_mode == false)
for(std::map<std::string, Dynamixel *>::iterator dxl_it = robot->dxls.begin(); dxl_it != robot->dxls.end(); dxl_it++)
{
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
ControlTableItem *_item = _dxl->bulk_read_items[_i];
if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true)
{
_data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length);
UINT32_T _data = 0;
// TODO: change dxl_state
if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name)
_dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset
else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name)
_dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data);
else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name)
_dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data);
else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name)
_dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset
else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name)
_dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data);
else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name)
_dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data);
else
_dxl->dxl_state->bulk_read_table[_item->item_name] = _data;
std::string _port_name = dxl_it->second->port_name;
std::string _joint_name = dxl_it->first;
Dynamixel *_dxl = dxl_it->second;
_present_state.header.stamp = ros::Time::now();
_goal_state.header.stamp = _present_state.header.stamp;
if(gazebo_mode == false && _dxl->bulk_read_items.size() > 0)
{
for(int _i = 0; _i < _dxl->bulk_read_items.size(); _i++)
{
ControlTableItem *_item = _dxl->bulk_read_items[_i];
if(port_to_bulk_read[_port_name]->IsAvailable(_dxl->id, _item->address, _item->data_length) == true)
{
_data = port_to_bulk_read[_port_name]->GetData(_dxl->id, _item->address, _item->data_length);
// TODO: change dxl_state
if(_dxl->present_position_item != 0 && _item->item_name == _dxl->present_position_item->item_name)
_dxl->dxl_state->present_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset
else if(_dxl->present_velocity_item != 0 && _item->item_name == _dxl->present_velocity_item->item_name)
_dxl->dxl_state->present_velocity = _dxl->ConvertValue2Velocity(_data);
else if(_dxl->present_current_item != 0 && _item->item_name == _dxl->present_current_item->item_name)
_dxl->dxl_state->present_current = _dxl->ConvertValue2Current(_data);
else if(_dxl->goal_position_item != 0 && _item->item_name == _dxl->goal_position_item->item_name)
_dxl->dxl_state->goal_position = _dxl->ConvertValue2Radian(_data) - _dxl->dxl_state->position_offset; // remove offset
else if(_dxl->goal_velocity_item != 0 && _item->item_name == _dxl->goal_velocity_item->item_name)
_dxl->dxl_state->goal_velocity = _dxl->ConvertValue2Velocity(_data);
else if(_dxl->goal_current_item != 0 && _item->item_name == _dxl->goal_current_item->item_name)
_dxl->dxl_state->goal_current = _dxl->ConvertValue2Current(_data);
else
_dxl->dxl_state->bulk_read_table[_item->item_name] = _data;
}
}
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
_dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec);
}
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
_dxl->dxl_state->update_time_stamp = TimeStamp(_present_state.header.stamp.sec, _present_state.header.stamp.nsec);
_present_state.name.push_back(_joint_name);
// TODO: should be converted to rad, rad/s, Nm
_present_state.position.push_back(_dxl->dxl_state->present_position);
_present_state.velocity.push_back(_dxl->dxl_state->present_velocity);
_present_state.effort.push_back(_dxl->dxl_state->present_current);
_goal_state.name.push_back(_joint_name);
_goal_state.position.push_back(_dxl->dxl_state->goal_position);
_goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity);
_goal_state.effort.push_back(_dxl->dxl_state->goal_current);
}
_present_state.name.push_back(_joint_name);
// TODO: should be converted to rad, rad/s, Nm
_present_state.position.push_back(_dxl->dxl_state->present_position);
_present_state.velocity.push_back(_dxl->dxl_state->present_velocity);
_present_state.effort.push_back(_dxl->dxl_state->present_current);
_goal_state.name.push_back(_joint_name);
_goal_state.position.push_back(_dxl->dxl_state->goal_position);
_goal_state.velocity.push_back(_dxl->dxl_state->goal_velocity);
_goal_state.effort.push_back(_dxl->dxl_state->goal_current);
}
// -> publish present joint_states & goal joint states topic
present_joint_state_pub.publish(_present_state);
goal_joint_state_pub.publish(_goal_state);
if(robot->sensors.size() > 0)
{
for(std::map<std::string, Sensor *>::iterator sensor_it = robot->sensors.begin(); sensor_it != robot->sensors.end(); sensor_it++)
{
UINT32_T _data = 0;
std::string _port_name = sensor_it->second->port_name;
std::string _sensor_name = sensor_it->first;
Sensor *_sensor = sensor_it->second;
if(gazebo_mode == false && _sensor->bulk_read_items.size() > 0)
{
for(int _i = 0; _i < _sensor->bulk_read_items.size(); _i++)
{
ControlTableItem *_item = _sensor->bulk_read_items[_i];
if(port_to_bulk_read[_port_name]->IsAvailable(_sensor->id, _item->address, _item->data_length) == true)
{
_data = port_to_bulk_read[_port_name]->GetData(_sensor->id, _item->address, _item->data_length);
// TODO: change sensor_state
_sensor->sensor_state->bulk_read_table[_item->item_name] = _data;
}
}
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
ros::Time _now = ros::Time::now();
_sensor->sensor_state->update_time_stamp = TimeStamp(_now.sec, _now.nsec);
}
}
}
// Call SensorModule Process()
// -> for loop : call SensorModule list -> Process()
if(sensor_modules_.size() > 0)
{
for(std::list<SensorModule*>::iterator _module_it = sensor_modules_.begin(); _module_it != sensor_modules_.end(); _module_it++)
{
(*_module_it)->Process(robot->dxls);
(*_module_it)->Process(robot->dxls, robot->sensors);
for(std::map<std::string, double>::iterator _it = (*_module_it)->result.begin(); _it != (*_module_it)->result.end(); _it++)
sensor_result_[_it->first] = _it->second;
@@ -1121,7 +1145,20 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module)
// set dxl's control module to "none"
for(std::map<std::string, Dynamixel*>::iterator _d_it = robot->dxls.begin(); _d_it != robot->dxls.end(); _d_it++)
{
_d_it->second->ctrl_module_name = "none";
Dynamixel *_dxl = _d_it->second;
_dxl->ctrl_module_name = "none";
UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset);
UINT8_T _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id);
port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id);
}
}
else
@@ -1132,12 +1169,57 @@ void RobotisController::SetCtrlModuleThread(std::string ctrl_module)
// if it exist
if((*_m_it)->module_name == ctrl_module)
{
CONTROL_MODE _mode = (*_m_it)->control_mode;
for(std::map<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result.begin(); _result_it != (*_m_it)->result.end(); _result_it++)
{
std::map<std::string, Dynamixel*>::iterator _d_it = robot->dxls.find(_result_it->first);
if(_d_it != robot->dxls.end())
{
_d_it->second->ctrl_module_name = ctrl_module;
Dynamixel *_dxl = _d_it->second;
_dxl->ctrl_module_name = ctrl_module;
if(_mode == POSITION_CONTROL)
{
UINT32_T _pos_data = _dxl->ConvertRadian2Value(_dxl->dxl_state->goal_position + _dxl->dxl_state->position_offset);
UINT8_T _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
port_to_sync_write_position[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id);
port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id);
}
else if(_mode == VELOCITY_CONTROL)
{
UINT32_T _vel_data = _dxl->ConvertVelocity2Value(_dxl->dxl_state->goal_velocity);
UINT8_T _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data));
port_to_sync_write_velocity[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
port_to_sync_write_torque[_dxl->port_name]->RemoveParam(_dxl->id);
port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id);
}
else if(_mode == CURRENT_CONTROL)
{
UINT32_T _curr_data = _dxl->ConvertCurrent2Value(_dxl->dxl_state->goal_current);
UINT8_T _sync_write_data[4];
_sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data));
_sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data));
_sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data));
_sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data));
port_to_sync_write_torque[_dxl->port_name]->AddParam(_dxl->id, _sync_write_data);
port_to_sync_write_velocity[_dxl->port_name]->RemoveParam(_dxl->id);
port_to_sync_write_position[_dxl->port_name]->RemoveParam(_dxl->id);
}
}
}

1
robotis_device/CMakeLists.txt
View File

@@ -39,6 +39,7 @@ include_directories(
## Declare a C++ library
add_library(robotis_device
src/robotis_device/Robot.cpp
src/robotis_device/Sensor.cpp
src/robotis_device/Dynamixel.cpp
)

77
robotis_device/devices/dynamixel/XM-430.device Normal file
View File

@@ -0,0 +1,77 @@
[device info]
model_name = XM-430
device_type = dynamixel
[type info]
current_ratio = 2.69
value_of_0_radian_position = 2048
value_of_min_radian_position = 0
value_of_max_radian_position = 4095
min_radian = -3.14159265
max_radian = 3.14159265
torque_enable_item_name = torque_enable
present_position_item_name = present_position
present_velocity_item_name = present_velocity
present_current_item_name = present_current
goal_position_item_name = goal_position
goal_velocity_item_name = goal_velocity
goal_current_item_name = goal_current
position_d_gain_item_name = position_d_gain
position_i_gain_item_name = position_i_gain
position_p_gain_item_name = position_p_gain
[control table]
# addr | item name | length | access | memory | min value | max value | signed
0 | model_number | 2 | R | EEPROM | 0 | 65535 | N
6 | version_of_firmware | 1 | R | EEPROM | 0 | 254 | N
7 | ID | 1 | RW | EEPROM | 0 | 252 | N
8 | baudrate | 1 | RW | EEPROM | 0 | 7 | N
9 | return_delay_time | 1 | RW | EEPROM | 0 | 254 | N
11 | operating_mode | 1 | RW | EEPROM | 0 | 16 | N
20 | homing_offset | 4 | RW | EEPROM | -1044479 | 1044479 | Y
24 | moving_threshold | 4 | RW | EEPROM | 0 | 1023 | N
31 | max_temperature_limit | 1 | RW | EEPROM | 0 | 100 | N
32 | max_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N
34 | min_voltage_limit | 2 | RW | EEPROM | 95 | 160 | N
36 | pwm_limit | 2 | RW | EEPROM | 0 | 885 | N
38 | current_limit | 2 | RW | EEPROM | 0 | 1193 | N
40 | acceleration_limit | 4 | RW | EEPROM | 0 | 32767 | N
44 | velocity_limit | 4 | RW | EEPROM | 0 | 1023 | N
48 | max_position_limit | 4 | RW | EEPROM | 0 | 4095 | N
52 | min_position_limit | 4 | RW | EEPROM | 0 | 4095 | N
63 | shutdown | 1 | RW | EEPROM | 0 | 255 | N
64 | torque_enable | 1 | RW | RAM | 0 | 1 | N
65 | LED | 1 | RW | RAM | 0 | 1 | N
68 | status_return_level | 1 | RW | RAM | 0 | 2 | N
69 | registered_instruction | 1 | R | RAM | 0 | 1 | N
70 | hardware_error_status | 1 | R | RAM | 0 | 255 | N
76 | velocity_i_gain | 2 | RW | RAM | 0 | 32767 | N
78 | velocity_p_gain | 2 | RW | RAM | 0 | 32767 | N
80 | position_d_gain | 2 | RW | RAM | 0 | 32767 | N
82 | position_i_gain | 2 | RW | RAM | 0 | 32767 | N
84 | position_p_gain | 2 | RW | RAM | 0 | 32767 | N
88 | feedforward_2nd_gain | 2 | RW | RAM | 0 | 32767 | N
90 | feedforward_1st_gain | 2 | RW | RAM | 0 | 32767 | N
100 | goal_pwm | 2 | RW | RAM | 0 | 885 | N
102 | goal_current | 2 | RW | RAM | 0 | 1193 | N
104 | goal_velocity | 4 | RW | RAM | 0 | 1023 | N
108 | profile_acceleration | 4 | RW | RAM | 0 | 32767 | N
112 | profile_velocity | 4 | RW | RAM | 0 | 1023 | N
116 | goal_position | 4 | RW | RAM | 0 | 4095 | N
120 | realtime_tick | 2 | R | RAM | 0 | 32767 | N
122 | moving | 1 | R | RAM | 0 | 1 | N
123 | moving_status | 1 | R | RAM | 0 | 255 | N
124 | present_pwm | 2 | R | RAM | 0 | 885 | N
126 | present_current | 2 | R | RAM | 0 | 1193 | N
128 | present_velocity | 4 | R | RAM | 0 | 1023 | N
132 | present_position | 4 | R | RAM | 0 | 4095 | N
136 | velocity_trajectory | 4 | R | RAM | 0 | 1023 | N
140 | position_trajectory | 4 | R | RAM | 0 | 4095 | N
144 | present_input_voltage | 2 | R | RAM | 95 | 160 | N
146 | present_temperature | 1 | R | RAM | 0 | 100 | N
168 | indirect_address_1 | 2 | RW | RAM | 0 | 65535 | N
224 | indirect_data_1 | 1 | RW | RAM | 0 | 255 | N
578 | indirect_address_29 | 2 | RW | RAM | 0 | 65535 | N
634 | indirect_data_29 | 1 | RW | RAM | 0 | 255 | N

25
robotis_device/devices/sensor/CM-740.device Normal file
View File

@@ -0,0 +1,25 @@
[device info]
model_name = CM-740
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_5 | 2 | RW | RAM | 0 | 32767 | N
28 | LED_6 | 2 | RW | RAM | 0 | 32767 | N
30 | button | 1 | RW | RAM | 0 | 3 | N
38 | gyro_z | 2 | R | RAM | 0 | 1023 | N
40 | gyro_y | 2 | R | RAM | 0 | 1023 | N
42 | gyro_x | 2 | R | RAM | 0 | 1023 | N
44 | acc_x | 2 | R | RAM | 0 | 1023 | N
46 | acc_y | 2 | R | RAM | 0 | 1023 | N
48 | acc_z | 2 | R | RAM | 0 | 1023 | N
50 | present_voltage | 1 | R | RAM | 50 | 250 | N

37
robotis_device/include/robotis_device/Device.h Normal file
View File

@@ -0,0 +1,37 @@
/*
* Device.h
*
* Created on: 2016. 5. 12.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_
#include <map>
#include <string>
#include <vector>
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Device
{
public:
UINT8_T id;
float protocol_version;
std::string model_name;
std::string port_name;
std::map<std::string, ControlTableItem *> ctrl_table;
std::vector<ControlTableItem *> bulk_read_items;
virtual ~Device() { }
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DEVICE_H_ */

19
robotis_device/include/robotis_device/Dynamixel.h
View File

@@ -5,29 +5,23 @@
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_
#include <map>
#include <vector>
#include <string>
#include "Device.h"
#include "DynamixelState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Dynamixel
class Dynamixel : public Device
{
public:
UINT8_T id;
std::string model_name;
float protocol_version;
std::map<std::string, ControlTableItem *> ctrl_table; // string: item name
std::string port_name;
std::string ctrl_module_name;
DynamixelState *dxl_state;
@@ -48,9 +42,6 @@ public:
ControlTableItem *goal_velocity_item;
ControlTableItem *goal_current_item;
std::vector<ControlTableItem *> bulk_read_items;
std::map<std::string, UINT16_T> indirect_address_table;
Dynamixel(int id, std::string model_name, float protocol_version);
double ConvertValue2Radian(INT32_T value);
@@ -66,4 +57,4 @@ public:
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXEL_H_ */
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXEL_H_ */

17
robotis_device/include/robotis_device/DynamixelState.h
View File

@@ -5,11 +5,13 @@
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_
#include <stdint.h>
#include <robotis_framework_common/RobotisDef.h>
#include "robotis_device/TimeStamp.h"
#include "robotis_framework_common/RobotisDef.h"
#define INDIRECT_DATA_1 "indirect_data_1"
#define INDIRECT_ADDRESS_1 "indirect_address_1"
@@ -17,13 +19,6 @@
namespace ROBOTIS
{
class TimeStamp {
public:
long sec;
long nsec;
TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { }
};
class DynamixelState
{
public:
@@ -57,4 +52,4 @@ public:
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_CONTROLLER_INCLUDE_DEVICE_DYNAMIXELSTATE_H_ */
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_DYNAMIXELSTATE_H_ */

5
robotis_device/include/robotis_device/Robot.h
View File

@@ -21,14 +21,15 @@ class Robot
{
public:
std::map<std::string, PortHandler *> ports; // string: port name
std::map<std::string, std::string> port_default_joint; // port name, default joint name
std::map<std::string, std::string> port_default_device; // port name, default device name
std::map<std::string, Dynamixel *> dxls; // string: joint name
std::map<std::string, Sensor *> sensors; // string: sensor name
Robot(std::string robot_file_path, std::string dev_desc_dir_path);
Dynamixel *getDynamixel(std::string path, int id, std::string port, float protocol_version);
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);
};
}

16
robotis_device/include/robotis_device/Sensor.h
View File

@@ -5,25 +5,23 @@
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_
#define ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_
#include <map>
#include <string>
#include <stdint.h>
#include "Device.h"
#include "SensorState.h"
#include "ControlTableItem.h"
namespace ROBOTIS
{
class Sensor
class Sensor : public Device
{
public:
UINT8_T id;
std::string model_name;
float protocol_version;
std::map<UINT16_T, ControlTableItem *> ctrl_table;
SensorState *sensor_state;
Sensor(int id, std::string model_name, float protocol_version);
};
@@ -31,4 +29,4 @@ public:
}
#endif /* ROBOTIS_FRAMEWORK_ROBOTIS_DEVICE_INCLUDE_DEVICE_SENSOR_H_ */
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSOR_H_ */

35
robotis_device/include/robotis_device/SensorState.h Normal file
View File

@@ -0,0 +1,35 @@
/*
* SensorState.h
*
* Created on: 2016. 5. 16.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_
#include "robotis_device/TimeStamp.h"
#include "robotis_framework_common/RobotisDef.h"
namespace ROBOTIS
{
class SensorState
{
public:
TimeStamp update_time_stamp;
std::map<std::string, UINT32_T> bulk_read_table;
SensorState()
: update_time_stamp(0, 0)
{
bulk_read_table.clear();
}
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_SENSORSTATE_H_ */

25
robotis_device/include/robotis_device/TimeStamp.h Normal file
View File

@@ -0,0 +1,25 @@
/*
* TimeStamp.h
*
* Created on: 2016. 5. 16.
* Author: zerom
*/
#ifndef ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_
#define ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_
namespace ROBOTIS
{
class TimeStamp {
public:
long sec;
long nsec;
TimeStamp(long sec, long nsec) : sec(sec), nsec(nsec) { }
};
}
#endif /* ROBOTIS_DEVICE_INCLUDE_ROBOTIS_DEVICE_TIMESTAMP_H_ */

14
robotis_device/src/robotis_device/Dynamixel.cpp
View File

@@ -5,16 +5,12 @@
* Author: zerom
*/
#include "../include/robotis_device/Dynamixel.h"
#include "robotis_device/Dynamixel.h"
using namespace ROBOTIS;
Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
: id(id),
model_name(model_name),
port_name(""),
ctrl_module_name("none"),
protocol_version(protocol_version),
: ctrl_module_name("none"),
current_ratio(1.0),
velocity_ratio(1.0),
value_of_0_radian_position(0),
@@ -30,11 +26,15 @@ Dynamixel::Dynamixel(int id, std::string model_name, float protocol_version)
goal_velocity_item(0),
goal_current_item(0)
{
this->id = id;
this->model_name = model_name;
this->port_name = "";
this->protocol_version = protocol_version;
ctrl_table.clear();
dxl_state = new DynamixelState();
bulk_read_items.clear();
indirect_address_table.clear();
}
double Dynamixel::ConvertValue2Radian(INT32_T value)

184
robotis_device/src/robotis_device/Robot.cpp
View File

@@ -78,7 +78,7 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
ports[tokens[0]] = (PortHandler*)PortHandler::GetPortHandler(tokens[0].c_str());
ports[tokens[0]]->SetBaudRate(std::atoi(tokens[1].c_str()));
port_default_joint[tokens[0]] = tokens[2];
port_default_device[tokens[0]] = tokens[2];
}
else if(session == "device info")
{
@@ -129,6 +129,49 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
}
else if(tokens[0] == "sensor")
{
std::string _file_path = dev_desc_dir_path + tokens[0] + "/" + tokens[3] + ".device";
int _id = std::atoi(tokens[2].c_str());
std::string _port = tokens[1];
float _protocol = std::atof(tokens[4].c_str());
std::string _dev_name = tokens[5];
sensors[_dev_name] = getSensor(_file_path, _id, _port, _protocol);
Sensor *_sensor = sensors[_dev_name];
std::vector<std::string> sub_tokens = split(tokens[6], ',');
if(sub_tokens.size() > 0)
{
std::map<std::string, ControlTableItem *>::iterator _indirect_it = _sensor->ctrl_table.find(INDIRECT_ADDRESS_1);
if(_indirect_it != _sensor->ctrl_table.end()) // INDIRECT_ADDRESS_1 exist
{
UINT16_T _indirect_data_addr = _sensor->ctrl_table[INDIRECT_DATA_1]->address;
for(int _i = 0; _i < sub_tokens.size(); _i++)
{
_sensor->bulk_read_items.push_back(new ControlTableItem());
ControlTableItem *_dest_item = _sensor->bulk_read_items[_i];
ControlTableItem *_src_item = _sensor->ctrl_table[sub_tokens[_i]];
_dest_item->item_name = _src_item->item_name;
_dest_item->address = _indirect_data_addr;
_dest_item->access_type = _src_item->access_type;
_dest_item->memory_type = _src_item->memory_type;
_dest_item->data_length = _src_item->data_length;
_dest_item->data_min_value = _src_item->data_min_value;
_dest_item->data_max_value = _src_item->data_max_value;
_dest_item->is_signed = _src_item->is_signed;
_indirect_data_addr += _dest_item->data_length;
}
}
else // INDIRECT_ADDRESS_1 exist
{
for(int _i = 0; _i < sub_tokens.size(); _i++)
_sensor->bulk_read_items.push_back(_sensor->ctrl_table[sub_tokens[_i]]);
}
}
}
}
}
file.close();
@@ -139,9 +182,92 @@ Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
}
}
Sensor *Robot::getSensor(std::string path, int id, std::string port, float protocol_version)
{
Sensor *_sensor;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
{
std::string _session = "";
std::string _input_str;
while(!file.eof())
{
std::getline(file, _input_str);
// remove comment ( # )
std::size_t pos = _input_str.find("#");
if(pos != std::string::npos)
_input_str = _input_str.substr(0, pos);
// trim
_input_str = trim(_input_str);
if(_input_str == "")
continue;
// find _session
if(!_input_str.compare(0, 1, "[") && !_input_str.compare(_input_str.size()-1, 1, "]"))
{
_input_str = _input_str.substr(1, _input_str.size()-2);
std::transform(_input_str.begin(), _input_str.end(), _input_str.begin(), ::tolower);
_session = trim(_input_str);
continue;
}
if(_session == "device info")
{
std::vector<std::string> tokens = split(_input_str, '=');
if(tokens.size() != 2)
continue;
if(tokens[0] == "model_name")
_sensor = new Sensor(id, tokens[1], protocol_version);
}
else if(_session == "control table")
{
std::vector<std::string> tokens = split(_input_str, '|');
if(tokens.size() != 8)
continue;
ControlTableItem *item = new ControlTableItem();
item->item_name = tokens[1];
item->address = std::atoi(tokens[0].c_str());
item->data_length = std::atoi(tokens[2].c_str());
if(tokens[3] == "R")
item->access_type = READ;
else if(tokens[3] == "RW")
item->access_type = READ_WRITE;
if(tokens[4] == "EEPROM")
item->memory_type = EEPROM;
else if(tokens[4] == "RAM")
item->memory_type = RAM;
item->data_min_value = std::atol(tokens[5].c_str());
item->data_max_value = std::atol(tokens[6].c_str());
if(tokens[7] == "Y")
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
_sensor->ctrl_table[tokens[1]] = item;
}
}
_sensor->port_name = port;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _sensor->id, _sensor->model_name.c_str());
//std::cout << "[ID:" << (int)(_sensor->id) << "] " << _sensor->model_name << " added. (" << port << ")" << std::endl;
file.close();
}
else
std::cout << "Unable to open file : " + path << std::endl;
return _sensor;
}
Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float protocol_version)
{
Dynamixel *dxl;
Dynamixel *_dxl;
// load file model_name.device
std::ifstream file(path.c_str());
if(file.is_open())
@@ -187,7 +313,7 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
continue;
if(tokens[0] == "model_name")
dxl = new Dynamixel(id, tokens[1], protocol_version);
_dxl = new Dynamixel(id, tokens[1], protocol_version);
}
else if(_session == "type info")
{
@@ -196,20 +322,20 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
continue;
if(tokens[0] == "current_ratio")
dxl->current_ratio = std::atof(tokens[1].c_str());
_dxl->current_ratio = std::atof(tokens[1].c_str());
else if(tokens[0] == "velocity_ratio")
dxl->velocity_ratio = std::atof(tokens[1].c_str());
_dxl->velocity_ratio = std::atof(tokens[1].c_str());
else if(tokens[0] == "value_of_0_radian_position")
dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str());
_dxl->value_of_0_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_min_radian_position")
dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str());
_dxl->value_of_min_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "value_of_max_radian_position")
dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str());
_dxl->value_of_max_radian_position = std::atoi(tokens[1].c_str());
else if(tokens[0] == "min_radian")
dxl->min_radian = std::atof(tokens[1].c_str());
_dxl->min_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "max_radian")
dxl->max_radian = std::atof(tokens[1].c_str());
_dxl->max_radian = std::atof(tokens[1].c_str());
else if(tokens[0] == "torque_enable_item_name")
_torque_enable_item_name = tokens[1];
@@ -250,33 +376,33 @@ Dynamixel *Robot::getDynamixel(std::string path, int id, std::string port, float
item->is_signed = true;
else if(tokens[7] == "N")
item->is_signed = false;
dxl->ctrl_table[tokens[1]] = item;
_dxl->ctrl_table[tokens[1]] = item;
}
}
dxl->port_name = port;
_dxl->port_name = port;
if(dxl->ctrl_table[_torque_enable_item_name] != NULL)
dxl->torque_enable_item = dxl->ctrl_table[_torque_enable_item_name];
if(dxl->ctrl_table[_present_position_item_name] != NULL)
dxl->present_position_item = dxl->ctrl_table[_present_position_item_name];
if(dxl->ctrl_table[_present_velocity_item_name] != NULL)
dxl->present_velocity_item = dxl->ctrl_table[_present_velocity_item_name];
if(dxl->ctrl_table[_present_current_item_name] != NULL)
dxl->present_current_item = dxl->ctrl_table[_present_current_item_name];
if(dxl->ctrl_table[_goal_position_item_name] != NULL)
dxl->goal_position_item = dxl->ctrl_table[_goal_position_item_name];
if(dxl->ctrl_table[_goal_velocity_item_name] != NULL)
dxl->goal_velocity_item = dxl->ctrl_table[_goal_velocity_item_name];
if(dxl->ctrl_table[_goal_current_item_name] != NULL)
dxl->goal_current_item = dxl->ctrl_table[_goal_current_item_name];
if(_dxl->ctrl_table[_torque_enable_item_name] != NULL)
_dxl->torque_enable_item = _dxl->ctrl_table[_torque_enable_item_name];
if(_dxl->ctrl_table[_present_position_item_name] != NULL)
_dxl->present_position_item = _dxl->ctrl_table[_present_position_item_name];
if(_dxl->ctrl_table[_present_velocity_item_name] != NULL)
_dxl->present_velocity_item = _dxl->ctrl_table[_present_velocity_item_name];
if(_dxl->ctrl_table[_present_current_item_name] != NULL)
_dxl->present_current_item = _dxl->ctrl_table[_present_current_item_name];
if(_dxl->ctrl_table[_goal_position_item_name] != NULL)
_dxl->goal_position_item = _dxl->ctrl_table[_goal_position_item_name];
if(_dxl->ctrl_table[_goal_velocity_item_name] != NULL)
_dxl->goal_velocity_item = _dxl->ctrl_table[_goal_velocity_item_name];
if(_dxl->ctrl_table[_goal_current_item_name] != NULL)
_dxl->goal_current_item = _dxl->ctrl_table[_goal_current_item_name];
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->id, dxl->model_name.c_str());
//std::cout << "[ID:" << (int)(dxl->id) << "] " << dxl->model_name << " added. (" << port << ")" << std::endl;
fprintf(stderr, "(%s) [ID:%3d] %14s added. \n", port.c_str(), _dxl->id, _dxl->model_name.c_str());
//std::cout << "[ID:" << (int)(_dxl->id) << "] " << _dxl->model_name << " added. (" << port << ")" << std::endl;
file.close();
}
else
std::cout << "Unable to open file : " + path << std::endl;
return dxl;
return _dxl;
}

23
robotis_device/src/robotis_device/Sensor.cpp Normal file
View File

@@ -0,0 +1,23 @@
/*
* Sensor.cpp
*
* Created on: 2016. 5. 11.
* Author: zerom
*/
#include "robotis_device/Sensor.h"
using namespace ROBOTIS;
Sensor::Sensor(int id, std::string model_name, float protocol_version)
{
this->id = id;
this->model_name = model_name;
this->port_name = "";
this->protocol_version = protocol_version;
ctrl_table.clear();
sensor_state = new SensorState();
bulk_read_items.clear();
}

1
robotis_framework_common/include/robotis_framework_common/MotionModule.h
View File

@@ -14,6 +14,7 @@
#include "robotis_device/Robot.h"
#include "robotis_device/Dynamixel.h"
#include "robotis_framework_common/Singleton.h"
namespace ROBOTIS
{

3
robotis_framework_common/include/robotis_framework_common/SensorModule.h
View File

@@ -14,6 +14,7 @@
#include "robotis_device/Robot.h"
#include "robotis_device/Dynamixel.h"
#include "robotis_framework_common/Singleton.h"
namespace ROBOTIS
{
@@ -28,7 +29,7 @@ public:
virtual ~SensorModule() { }
virtual void Initialize(const int control_cycle_msec, Robot *robot) = 0;
virtual void Process(std::map<std::string, Dynamixel *> dxls) = 0;
virtual void Process(std::map<std::string, Dynamixel *> dxls, std::map<std::string, Sensor *> sensors) = 0;
};
}

49
robotis_framework_common/include/robotis_framework_common/Singleton.h Normal file
View File

@@ -0,0 +1,49 @@
/*
* Singleton.h
*
* Created on: 2016. 5. 17.
* Author: zerom
*/
#ifndef ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_
#define ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_
namespace ROBOTIS
{
template <class T>
class Singleton
{
private:
static T *unique_instance_;
protected:
Singleton() { }
Singleton(Singleton const&) { }
Singleton& operator=(Singleton const&) { return *this; }
public:
static T* GetInstance()
{
if(unique_instance_ == NULL)
unique_instance_ = new T;
return unique_instance_;
}
static void DestroyInstance()
{
if(unique_instance_)
{
delete unique_instance_;
unique_instance_ = NULL;
}
}
};
template <class T> T* Singleton<T>::unique_instance_ = NULL;
}
#endif /* ROBOTIS_FRAMEWORK_COMMON_INCLUDE_ROBOTIS_FRAMEWORK_COMMON_SINGLETON_H_ */