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

Browse Source 
merge for sync kinetic-devel and master branch
This commit is contained in:
Yoonseok Pyo
2017-08-09 15:23:46 +09:00
committed by GitHub
parent f08d522e0b da6ed0eece
commit 963f502ccf
2 changed files with 24 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
robotis_controller/src/robotis_controller/robotis_controller.cpp
View File

@@ -911,12 +911,14 @@ void RobotisController::process()
if (dxl->bulk_read_items_.size() > 0)
{
uint32_t data = 0;
bool updated = false;
uint32_t data = 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)
{
updated = true;
data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_);
// change dxl_state
@@ -938,7 +940,8 @@ void RobotisController::process()
}
// -> 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);
if (updated == true)
dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
}
}
}
@@ -954,12 +957,14 @@ void RobotisController::process()
if (sensor->bulk_read_items_.size() > 0)
{
uint32_t data = 0;
bool updated = false;
uint32_t data = 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)
{
updated = true;
data = port_to_bulk_read_[port_name]->getData(sensor->id_, item->address_, item->data_length_);
// change sensor_state
@@ -968,7 +973,8 @@ void RobotisController::process()
}
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
if (updated == true)
sensor->sensor_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
}
}
}
@@ -1463,9 +1469,11 @@ void RobotisController::removeSensorModule(SensorModule *module)
void RobotisController::writeControlTableCallback(const robotis_controller_msgs::WriteControlTable::ConstPtr &msg)
{
fprintf(stderr, "[WriteControlTable] led control msg received\n");
Device *device = NULL;
if (DEBUG_PRINT)
fprintf(stderr, "[WriteControlTable] led control msg received\n");
auto dev_it1 = robot_->dxls_.find(msg->joint_name);
if(dev_it1 != robot_->dxls_.end())
{
@@ -1484,7 +1492,7 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs:
return;
}
}
fprintf(stderr, " #1 ");
ControlTableItem *item = NULL;
auto item_it = device->ctrl_table_.find(msg->start_item_name);
if(item_it != device->ctrl_table_.end())
@@ -1493,11 +1501,10 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs:
}
else
{
ROS_WARN("WriteControlTable] Unknown item : %s", msg->start_item_name.c_str());
ROS_WARN("[WriteControlTable] Unknown item : %s", msg->start_item_name.c_str());
return;
}
fprintf(stderr, " #2 ");
dynamixel::PortHandler *port = robot_->ports_[device->port_name_];
dynamixel::PacketHandler *packet_handler = dynamixel::PacketHandler::getPacketHandler(device->protocol_version_);
@@ -1506,17 +1513,13 @@ void RobotisController::writeControlTableCallback(const robotis_controller_msgs:
queue_mutex_.lock();
direct_sync_write_.push_back(new dynamixel::GroupSyncWrite(port, packet_handler, item->address_, msg->data_length));
fprintf(stderr, " #3 \n");
direct_sync_write_[direct_sync_write_.size() - 1]->addParam(device->id_, (uint8_t *)(msg->data.data()));
fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str());
for (auto &dt : msg->data)
fprintf(stderr, "%02X ", dt);
fprintf(stderr, "\n");
// fprintf(stderr, "[WriteControlTable] %s -> %s : ", msg->joint_name.c_str(), msg->start_item_name.c_str());
// for (auto &dt : msg->data)
// fprintf(stderr, "%02X ", dt);
// fprintf(stderr, "\n");
queue_mutex_.unlock();
@@ -1566,6 +1569,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
if (item->access_type_ == Read)
continue;
queue_mutex_.lock();
int idx = 0;
if (direct_sync_write_.size() == 0)
{
@@ -1601,6 +1606,8 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
}
direct_sync_write_[idx]->addParam(device->id_, data);
delete[] data;
queue_mutex_.unlock();
}
}

2
robotis_device/devices/sensor/OPEN-CR.device Normal file → Executable file
View File

@@ -10,7 +10,7 @@ device_type = sensor
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
24 | dynamixel_power | 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