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 #23 from ROBOTIS-GIT/master

Browse Source 
PR for minor release
This commit is contained in:
Yoonseok Pyo
2016-08-18 07:54:30 +09:00
committed by GitHub
parent ede458ace6 71aa71ec18
commit c7fca552c6
4 changed files with 133 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
robotis_controller/package.xml
View File

@ -2,7 +2,9 @@
<package>
<name>robotis_controller</name>
<version>0.1.0</version>
<description>The robotis_controller package</description>
<description>
The main package that controls THORMANG3.
</description>
<license>BSD</license>
<author email="zerom@robotis.com">zerom</author>
<maintainer email="pyo@robotis.com">pyo</maintainer>

229
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@ -348,7 +348,119 @@ bool RobotisController::initialize(const std::string robot_file_path, const std:
initializeDevice(init_file_path);
queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this));
return true;
}
void RobotisController::initializeDevice(const std::string init_file_path)
{
// device initialize
if (DEBUG_PRINT)
ROS_WARN("INIT FILE LOAD");
YAML::Node doc;
try
{
doc = YAML::LoadFile(init_file_path.c_str());
for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++)
{
std::string joint_name = it_doc->first.as<std::string>();
YAML::Node joint_node = doc[joint_name];
if (joint_node.size() == 0)
continue;
Dynamixel *dxl = NULL;
auto dxl_it = robot_->dxls_.find(joint_name);
if (dxl_it != robot_->dxls_.end())
dxl = dxl_it->second;
if (dxl == NULL)
{
ROS_WARN("Joint [%s] was not found.", joint_name.c_str());
continue;
}
if (DEBUG_PRINT)
ROS_INFO("JOINT_NAME: %s", joint_name.c_str());
for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++)
{
std::string item_name = it_joint->first.as<std::string>();
if (DEBUG_PRINT)
ROS_INFO(" ITEM_NAME: %s", item_name.c_str());
uint32_t value = it_joint->second.as<uint32_t>();
ControlTableItem *item = dxl->ctrl_table_[item_name];
if (item == NULL)
{
ROS_WARN("Control Item [%s] was not found.", item_name.c_str());
continue;
}
if (item->memory_type_ == EEPROM)
{
uint8_t data8 = 0;
uint16_t data16 = 0;
uint32_t data32 = 0;
switch (item->data_length_)
{
case 1:
read1Byte(joint_name, item->address_, &data8);
if (data8 == value)
continue;
break;
case 2:
read2Byte(joint_name, item->address_, &data16);
if (data16 == value)
continue;
break;
case 4:
read4Byte(joint_name, item->address_, &data32);
if (data32 == value)
continue;
break;
default:
break;
}
}
switch (item->data_length_)
{
case 1:
write1Byte(joint_name, item->address_, (uint8_t) value);
break;
case 2:
write2Byte(joint_name, item->address_, (uint16_t) value);
break;
case 4:
write4Byte(joint_name, item->address_, value);
break;
default:
break;
}
if (item->memory_type_ == EEPROM)
{
// Write to EEPROM -> delay is required (max delay: 55 msec per byte)
usleep(item->data_length_ * 55 * 1000);
}
}
}
} catch (const std::exception& e)
{
ROS_INFO("Dynamixel Init file not found.");
}
// [ BulkRead ] StartAddress : Present Position , Length : 10 ( Position/Velocity/Current )
for (auto& it : robot_->ports_)
{
if (port_to_bulk_read_[it.first] != 0)
port_to_bulk_read_[it.first]->clearParam();
}
for (auto& it : robot_->dxls_)
{
std::string joint_name = it.first;
@ -487,113 +599,6 @@ bool RobotisController::initialize(const std::string robot_file_path, const std:
if (bulkread_start_addr != 0)
port_to_bulk_read_[sensor->port_name_]->addParam(sensor->id_, bulkread_start_addr, bulkread_data_length);
}
queue_thread_ = boost::thread(boost::bind(&RobotisController::msgQueueThread, this));
return true;
}
void RobotisController::initializeDevice(const std::string init_file_path)
{
// device initialize
if (DEBUG_PRINT)
ROS_WARN("INIT FILE LOAD");
YAML::Node doc;
try
{
doc = YAML::LoadFile(init_file_path.c_str());
for (YAML::const_iterator it_doc = doc.begin(); it_doc != doc.end(); it_doc++)
{
std::string joint_name = it_doc->first.as<std::string>();
YAML::Node joint_node = doc[joint_name];
if (joint_node.size() == 0)
continue;
Dynamixel *dxl = NULL;
auto dxl_it = robot_->dxls_.find(joint_name);
if (dxl_it != robot_->dxls_.end())
dxl = dxl_it->second;
if (dxl == NULL)
{
ROS_WARN("Joint [%s] was not found.", joint_name.c_str());
continue;
}
if (DEBUG_PRINT)
ROS_INFO("JOINT_NAME: %s", joint_name.c_str());
for (YAML::const_iterator it_joint = joint_node.begin(); it_joint != joint_node.end(); it_joint++)
{
std::string item_name = it_joint->first.as<std::string>();
if (DEBUG_PRINT)
ROS_INFO(" ITEM_NAME: %s", item_name.c_str());
uint32_t value = it_joint->second.as<uint32_t>();
ControlTableItem *item = dxl->ctrl_table_[item_name];
if (item == NULL)
{
ROS_WARN("Control Item [%s] was not found.", item_name.c_str());
continue;
}
if (item->memory_type_ == EEPROM)
{
uint8_t data8 = 0;
uint16_t data16 = 0;
uint32_t data32 = 0;
switch (item->data_length_)
{
case 1:
read1Byte(joint_name, item->address_, &data8);
if (data8 == value)
continue;
break;
case 2:
read2Byte(joint_name, item->address_, &data16);
if (data16 == value)
continue;
break;
case 4:
read4Byte(joint_name, item->address_, &data32);
if (data32 == value)
continue;
break;
default:
break;
}
}
switch (item->data_length_)
{
case 1:
write1Byte(joint_name, item->address_, (uint8_t) value);
break;
case 2:
write2Byte(joint_name, item->address_, (uint16_t) value);
break;
case 4:
write4Byte(joint_name, item->address_, value);
break;
default:
break;
}
if (item->memory_type_ == EEPROM)
{
// Write to EEPROM -> delay is required (max delay: 55 msec per byte)
usleep(item->data_length_ * 55 * 1000);
}
}
}
} catch (const std::exception& e)
{
ROS_INFO("Dynamixel Init file not found.");
}
}
void RobotisController::gazeboTimerThread()
@ -1186,6 +1191,16 @@ void RobotisController::process()
// SyncWrite
if (gazebo_mode_ == false && do_sync_write)
{
if (direct_sync_write_.size() > 0)
{
for (int i = 0; i < direct_sync_write_.size(); i++)
{
direct_sync_write_[i]->txPacket();
direct_sync_write_[i]->clearParam();
}
direct_sync_write_.clear();
}
if (port_to_sync_write_position_p_gain_.size() > 0)
{
for (auto& it : port_to_sync_write_position_p_gain_)

6
robotis_device/package.xml
View File

@ -2,7 +2,11 @@
<package>
<name>robotis_device</name>
<version>0.1.0</version>
<description>The robotis_device package</description>
<description>
The package that manages device information of ROBOTIS robots.
This package is used when reading device information with the robot information file
from the robotis_controller package.
</description>
<maintainer email="zerom@robotis.com">robotis</maintainer>

4
robotis_framework_common/package.xml
View File

@ -2,7 +2,9 @@
<package>
<name>robotis_framework_common</name>
<version>0.1.0</version>
<description>The robotis_framework_common package</description>
<description>
The package contains commonly used Headers for the ROBOTIS Framework.
</description>
<license>BSD</license>
<author email="zerom@robotis.com">zerom</author>
<maintainer email="pyo@robotis.com">pyo</maintainer>