Skip to content

Commit 35af44b

Browse files
kaym9nkaym9n
committed
Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-Framework into develop
2 parents 60dfda6 + 8660bd0 commit 35af44b

File tree

14 files changed

+365
-181
lines changed

14 files changed

+365
-181
lines changed

‎.gitignore‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1,11 @@
1-
/robotis_device/devices/dynamixel/H54-100-B210-R-NR.device
1+
build
2+
devel
3+
bin
4+
lib
5+
msg_gen
6+
srv_gen
7+
qtcreator-build
8+
*~
9+
*.backup
10+
*.user
11+
*.autosave

‎robotis_controller/CHANGELOG.rst‎

Lines changed: 64 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,64 @@
1-
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2-
Changelog for package robotis_controller
3-
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4-
5-
0.1.1 (2016-08-18)
6-
-----------
7-
* updated the package information
8-
9-
0.1.0 (2016-08-12)
10-
-----------
11-
* first public release for Kinetic
12-
* modified the package information for release
13-
* develop branch -> master branch
14-
* function name changed : DeviceInit() -> InitDevice()
15-
* Fixed high CPU consumption due to busy waits
16-
* add SensorState
17-
add Singleton template
18-
* XM-430 / CM-740 device file added.
19-
Sensor device added.
20-
* added code to support the gazebo simulator
21-
* added first bulk read failure protection code
22-
* renewal
23-
* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package robotis_controller
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
0.2.1 (2016-11-23)
6+
-----------
7+
* Merge the changes and update
8+
* - Direct Control Mode bug fixed.
9+
* update
10+
* - added writeControlTableCallback
11+
* - added WriteControlTable msg callback
12+
* mode change debugging
13+
* - optimized cpu usage by spin loop (by astumpf)
14+
* - robotis_controller process() : processing order changed.
15+
* 1st : packet communication
16+
* 2nd : processing modules
17+
* - dependencies fixed. (Pull requests `#26 <https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues/26>`_)
18+
* - make setJointCtrlModuleCallback() to the thread function & improved.
19+
* - modified dependency problem.
20+
* - reduce CPU consumption
21+
* Contributors: Jay Song, Pyo, Zerom, SCH
22+
23+
0.2.0 (2016-08-31)
24+
-----------
25+
* bug fixed (position pid gain & velocity pid gain sync write).
26+
* added velocity_to_value_ratio to DXL Pro-H series.
27+
* changed some debug messages.
28+
* added velocity p/i/d gain and position i/d gain sync_write code.
29+
* SyncWriteItem bug fixed.
30+
* add function / modified the code simple (using auto / range based for loop)
31+
* added XM-430-W210 / XM-430-W350 device file.
32+
* rename ControlMode(CurrentControl -> TorqueControl)
33+
* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_)
34+
* rename (present_current\_ -> present_torque\_)
35+
* modified torque control code
36+
* fixed typos / changed ROS_INFO -> fprintf (for processing speed)
37+
* startTimer() : after bulkread txpacket(), need some sleep()
38+
* changed the order of processing in the Process() function.
39+
* added missing mutex for gazebo
40+
* fixed crash when running in gazebo simulation
41+
* sync write bug fix.
42+
* added position_p_gain sync write
43+
* MotionModule/SensorModule member variable access changed (public -> protected).
44+
* Contributors: Jay Song, Zerom, Pyo, SCH
45+
46+
0.1.1 (2016-08-18)
47+
-----------
48+
* updated the package information
49+
50+
0.1.0 (2016-08-12)
51+
-----------
52+
* first public release for Kinetic
53+
* modified the package information for release
54+
* develop branch -> master branch
55+
* function name changed : DeviceInit() -> InitDevice()
56+
* Fixed high CPU consumption due to busy waits
57+
* add SensorState
58+
add Singleton template
59+
* XM-430 / CM-740 device file added.
60+
Sensor device added.
61+
* added code to support the gazebo simulator
62+
* added first bulk read failure protection code
63+
* renewal
64+
* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo

‎robotis_controller/include/robotis_controller/robotis_controller.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -95,8 +95,6 @@ class RobotisController : public Singleton<RobotisController>
9595
void initializeSyncWrite();
9696

9797
public:
98-
static const int CONTROL_CYCLE_MSEC = 8; // 8 msec
99-
10098
bool DEBUG_PRINT;
10199
Robot *robot_;
102100

‎robotis_controller/package.xml‎

Lines changed: 32 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,28 +1,32 @@
1-
<?xml version="1.0"?>
2-
<package>
3-
<name>robotis_controller</name>
4-
<version>0.1.1</version>
5-
<description>
6-
The main package that controls THORMANG3.
7-
</description>
8-
<license>BSD</license>
9-
<author email="[email protected]">Zerom</author>
10-
<maintainer email="[email protected]">Pyo</maintainer>
11-
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
12-
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
13-
<url type="website">http://wiki.ros.org/robotis_controller</url>
14-
<buildtool_depend>catkin</buildtool_depend>
15-
<build_depend>roscpp</build_depend>
16-
<build_depend>roslib</build_depend>
17-
<build_depend>std_msgs</build_depend>
18-
<build_depend>sensor_msgs</build_depend>
19-
<build_depend>robotis_controller_msgs</build_depend>
20-
<build_depend>robotis_framework_common</build_depend>
21-
<run_depend>roscpp</run_depend>
22-
<run_depend>roslib</run_depend>
23-
<run_depend>std_msgs</run_depend>
24-
<run_depend>sensor_msgs</run_depend>
25-
<run_depend>robotis_controller_msgs</run_depend>
26-
<run_depend>robotis_framework_common</run_depend>
27-
<export></export>
28-
</package>
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>robotis_controller</name>
4+
<version>0.2.1</version>
5+
<description>
6+
The main package that controls THORMANG3.
7+
</description>
8+
<license>BSD</license>
9+
<author email="[email protected]">Zerom</author>
10+
<maintainer email="[email protected]">Pyo</maintainer>
11+
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues</url>
12+
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-Framework</url>
13+
<url type="website">http://wiki.ros.org/robotis_controller</url>
14+
<buildtool_depend>catkin</buildtool_depend>
15+
<build_depend>roscpp</build_depend>
16+
<build_depend>roslib</build_depend>
17+
<build_depend>std_msgs</build_depend>
18+
<build_depend>sensor_msgs</build_depend>
19+
<build_depend>dynamixel_sdk</build_depend>
20+
<build_depend>robotis_device</build_depend>
21+
<build_depend>robotis_controller_msgs</build_depend>
22+
<build_depend>robotis_framework_common</build_depend>
23+
<run_depend>roscpp</run_depend>
24+
<run_depend>roslib</run_depend>
25+
<run_depend>std_msgs</run_depend>
26+
<run_depend>sensor_msgs</run_depend>
27+
<run_depend>dynamixel_sdk</run_depend>
28+
<run_depend>robotis_device</run_depend>
29+
<run_depend>robotis_controller_msgs</run_depend>
30+
<run_depend>robotis_framework_common</run_depend>
31+
<export></export>
32+
</package>

‎robotis_controller/src/robotis_controller/robotis_controller.cpp‎

Lines changed: 79 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -603,7 +603,7 @@ void RobotisController::initializeDevice(const std::string init_file_path)
603603

604604
void RobotisController::gazeboTimerThread()
605605
{
606-
ros::Rate gazebo_rate(1000 / CONTROL_CYCLE_MSEC);
606+
ros::Rate gazebo_rate(1000 / robot_->getControlCycle());
607607

608608
while (!stop_timer_)
609609
{
@@ -662,7 +662,7 @@ void RobotisController::msgQueueThread()
662662
ros::ServiceServer joint_module_server = ros_node.advertiseService("/robotis/get_present_joint_ctrl_modules",
663663
&RobotisController::getCtrlModuleCallback, this);
664664

665-
ros::WallDuration duration(CONTROL_CYCLE_MSEC / 1000.0);
665+
ros::WallDuration duration(robot_->getControlCycle() / 1000.0);
666666
while(ros_node.ok())
667667
callback_queue.callAvailable(duration);
668668
}
@@ -679,8 +679,8 @@ void *RobotisController::timerThread(void *param)
679679

680680
while (!controller->stop_timer_)
681681
{
682-
next_time.tv_sec += (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) / 1000000000;
683-
next_time.tv_nsec = (next_time.tv_nsec + CONTROL_CYCLE_MSEC * 1000000) % 1000000000;
682+
next_time.tv_sec += (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) / 1000000000;
683+
next_time.tv_nsec = (next_time.tv_nsec + controller->robot_->getControlCycle() * 1000000) % 1000000000;
684684

685685
controller->process();
686686

@@ -1109,14 +1109,64 @@ void RobotisController::process()
11091109
{
11101110
if(gazebo_mode_ == false)
11111111
{
1112-
queue_mutex_.lock();
1112+
// BulkRead Rx
1113+
for (auto& it : port_to_bulk_read_)
1114+
{
1115+
robot_->ports_[it.first]->setPacketTimeout(0.0);
1116+
it.second->rxPacket();
1117+
}
11131118

1114-
for (auto& it : port_to_sync_write_position_)
1119+
// -> save to robot->dxls_[]->dxl_state_
1120+
if (robot_->dxls_.size() > 0)
11151121
{
1116-
it.second->txPacket();
1117-
it.second->clearParam();
1122+
for (auto& dxl_it : robot_->dxls_)
1123+
{
1124+
Dynamixel *dxl = dxl_it.second;
1125+
std::string port_name = dxl_it.second->port_name_;
1126+
std::string joint_name = dxl_it.first;
1127+
1128+
if (dxl->bulk_read_items_.size() > 0)
1129+
{
1130+
uint32_t data = 0;
1131+
for (int i = 0; i < dxl->bulk_read_items_.size(); i++)
1132+
{
1133+
ControlTableItem *item = dxl->bulk_read_items_[i];
1134+
if (port_to_bulk_read_[port_name]->isAvailable(dxl->id_, item->address_, item->data_length_) == true)
1135+
{
1136+
data = port_to_bulk_read_[port_name]->getData(dxl->id_, item->address_, item->data_length_);
1137+
1138+
// change dxl_state
1139+
if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_)
1140+
dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
1141+
else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_)
1142+
dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data);
1143+
else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_)
1144+
dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data);
1145+
else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_)
1146+
dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
1147+
else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_)
1148+
dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
1149+
else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
1150+
dxl->dxl_state_->goal_torque_ = dxl->convertValue2Torque(data);
1151+
else
1152+
dxl->dxl_state_->bulk_read_table_[item->item_name_] = data;
1153+
}
1154+
}
1155+
1156+
// -> update time stamp to Robot->dxls[]->dynamixel_state.update_time_stamp
1157+
dxl->dxl_state_->update_time_stamp_ = TimeStamp(present_state.header.stamp.sec, present_state.header.stamp.nsec);
1158+
}
1159+
}
11181160
}
11191161

1162+
queue_mutex_.lock();
1163+
1164+
// for (auto& it : port_to_sync_write_position_)
1165+
// {
1166+
// it.second->txPacket();
1167+
// it.second->clearParam();
1168+
// }
1169+
11201170
if (direct_sync_write_.size() > 0)
11211171
{
11221172
for (int i = 0; i < direct_sync_write_.size(); i++)
@@ -1128,6 +1178,10 @@ void RobotisController::process()
11281178
}
11291179

11301180
queue_mutex_.unlock();
1181+
1182+
// BulkRead Tx
1183+
for (auto& it : port_to_bulk_read_)
1184+
it.second->txPacket();
11311185
}
11321186
}
11331187

@@ -1375,7 +1429,7 @@ void RobotisController::addMotionModule(MotionModule *module)
13751429
}
13761430
}
13771431

1378-
module->initialize(CONTROL_CYCLE_MSEC, robot_);
1432+
module->initialize(robot_->getControlCycle(), robot_);
13791433
motion_modules_.push_back(module);
13801434
motion_modules_.unique();
13811435
}
@@ -1397,7 +1451,7 @@ void RobotisController::addSensorModule(SensorModule *module)
13971451
}
13981452
}
13991453

1400-
module->initialize(CONTROL_CYCLE_MSEC, robot_);
1454+
module->initialize(robot_->getControlCycle(), robot_);
14011455
sensor_modules_.push_back(module);
14021456
sensor_modules_.unique();
14031457
}
@@ -1553,9 +1607,22 @@ void RobotisController::syncWriteItemCallback(const robotis_controller_msgs::Syn
15531607
void RobotisController::setControllerModeCallback(const std_msgs::String::ConstPtr &msg)
15541608
{
15551609
if (msg->data == "DirectControlMode")
1610+
{
1611+
for (auto& it : port_to_bulk_read_)
1612+
{
1613+
robot_->ports_[it.first]->setPacketTimeout(0.0);
1614+
it.second->rxPacket();
1615+
}
15561616
controller_mode_ = DirectControlMode;
1617+
}
15571618
else if (msg->data == "MotionModuleMode")
1619+
{
1620+
for (auto& it : port_to_bulk_read_)
1621+
{
1622+
it.second->txPacket();
1623+
}
15581624
controller_mode_ = MotionModuleMode;
1625+
}
15591626
}
15601627

15611628
void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
@@ -1664,7 +1731,7 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::
16641731
for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
16651732
{
16661733
while((*_stop_m_it)->isRunning())
1667-
usleep(CONTROL_CYCLE_MSEC * 1000);
1734+
usleep(robot_->getControlCycle() * 1000);
16681735
}
16691736

16701737
// disable module(s)
@@ -1879,7 +1946,7 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
18791946
for (auto stop_m_it = stop_modules.begin(); stop_m_it != stop_modules.end(); stop_m_it++)
18801947
{
18811948
while ((*stop_m_it)->isRunning())
1882-
usleep(CONTROL_CYCLE_MSEC * 1000);
1949+
usleep(robot_->getControlCycle() * 1000);
18831950
}
18841951

18851952
// disable module(s)

0 commit comments

Comments
 (0)