Skip to content

Commit 06e5e17

Browse files
Shota Aokiknjinki
Shota Aoki
and
knjinki
authored
parse_config_file関数をhardwareクラスから抽出する (#16)
* config_file_paserの作成。無効なファイルパスを開かない * コンフィグファイルの読み込みと1ジョイントのテストを実装。リミットの処理に残課題あり * ジョイントのリミットとマージンを関数でセットできるように変更 * config_file_parserでread, writeグループの検証をする。 テスト項目とコンフィグファイルを追加 * parse_config_file関数をhardwareから削除し、代わりにconfig_file_parser::parse関数を使用する * Jointクラスから不要なコンストラクタを削除 * Update rt_manipulators_lib/include/config_file_parser.hpp Co-authored-by: kenjiinukai <[email protected]> Co-authored-by: kenjiinukai <[email protected]>
1 parent bd581f3 commit 06e5e17

21 files changed

+475
-155
lines changed

‎rt_manipulators_lib/include/config_file_parser.hpp

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
// Copyright 2022 RT Corporation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RT_MANIPULATORS_LIB_INCLUDE_CONFIG_FILE_PARSER_HPP_
16+
#define RT_MANIPULATORS_LIB_INCLUDE_CONFIG_FILE_PARSER_HPP_
17+
18+
#include <string>
19+
#include "hardware_joints.hpp"
20+
21+
namespace config_file_parser {
22+
23+
using JointName = std::string;
24+
25+
bool parse(const std::string& config_yaml, hardware_joints::Joints & parsed_joints);
26+
27+
} // namespace config_file_parser
28+
29+
#endif // RT_MANIPULATORS_LIB_INCLUDE_CONFIG_FILE_PARSER_HPP_

‎rt_manipulators_lib/include/hardware.hpp

-1
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,6 @@ class Hardware {
9696
std::shared_ptr<hardware_communicator::Communicator> comm_;
9797

9898
private:
99-
bool parse_config_file(const std::string& config_yaml);
10099
bool write_operating_mode(const std::string& group_name);
101100
bool limit_goal_velocity_by_present_position(const std::string& group_name);
102101
bool limit_goal_current_by_present_position(const std::string& group_name);

‎rt_manipulators_lib/include/joint.hpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -23,13 +23,15 @@ namespace joint {
2323

2424
class Joint {
2525
public:
26-
Joint(const uint8_t id, const uint8_t operating_mode,
27-
const double max_position_limit, const double min_position_limit,
28-
const double current_limit_when_position_exceeds_limit);
26+
Joint(const uint8_t id, const uint8_t operating_mode);
2927
uint8_t id() const;
3028
uint8_t operating_mode() const;
29+
void set_position_limit_margin(const double position_radian);
30+
void set_position_limit(const double min_position_radian, const double max_position_radian);
3131
double max_position_limit() const;
3232
double min_position_limit() const;
33+
void set_current_limit_margin(const double current_ampere);
34+
void set_current_limit(const double max_current_ampere);
3335
double current_limit_when_position_exceeds_limit() const;
3436
void set_present_position(const double position_radian);
3537
void set_present_velocity(const double velocity_rps);
@@ -51,8 +53,11 @@ class Joint {
5153
private:
5254
uint8_t id_;
5355
uint8_t operating_mode_;
56+
double position_limit_margin_;
5457
double max_position_limit_;
5558
double min_position_limit_;
59+
double current_limit_margin_;
60+
double max_current_limit_;
5661
double current_limit_when_position_exceeds_limit_;
5762
double present_position_;
5863
double present_velocity_;

‎rt_manipulators_lib/src/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ add_library(${library_name}
1111
hardware_communicator.cpp
1212
kinematics.cpp
1313
kinematics_utils.cpp
14+
config_file_parser.cpp
1415
)
1516
set_target_properties(${library_name} PROPERTIES VERSION 1.0.0 SOVERSION 1)
1617

‎rt_manipulators_lib/src/config_file_parser.cpp

+115
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
// Copyright 2022 RT Corporation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
16+
#include <yaml-cpp/yaml.h>
17+
#include <fstream>
18+
#include <iostream>
19+
20+
#include "config_file_parser.hpp"
21+
#include "joint.hpp"
22+
23+
24+
namespace config_file_parser {
25+
26+
bool parse(const std::string& config_yaml, hardware_joints::Joints & parsed_joints) {
27+
std::ifstream fs(config_yaml);
28+
if (!fs.is_open()) {
29+
std::cerr << "コンフィグファイル:" << config_yaml << "が存在しません." << std::endl;
30+
return false;
31+
}
32+
33+
YAML::Node config = YAML::LoadFile(config_yaml);
34+
for (const auto & config_joint_group : config["joint_groups"]) {
35+
auto group_name = config_joint_group.first.as<std::string>();
36+
if (parsed_joints.has_group(group_name)) {
37+
std::cerr << group_name << "グループが2つ以上存在します." << std::endl;
38+
return false;
39+
}
40+
41+
if (!config["joint_groups"][group_name]["joints"]) {
42+
std::cerr << group_name << "グループに'joints'が設定されていせん。" << std::endl;
43+
return false;
44+
}
45+
46+
std::vector<JointName> joint_names;
47+
for (const auto & config_joint : config["joint_groups"][group_name]["joints"]) {
48+
auto joint_name = config_joint.as<std::string>();
49+
if (parsed_joints.has_joint(joint_name)) {
50+
std::cerr << joint_name << "ジョイントが2つ以上存在します." << std::endl;
51+
return false;
52+
}
53+
54+
if (!config[joint_name]) {
55+
std::cerr << joint_name << "ジョイントの設定が存在しません." << std::endl;
56+
return false;
57+
}
58+
59+
if (!config[joint_name]["id"] || !config[joint_name]["operating_mode"]) {
60+
std::cerr << joint_name << "にidまたはoperating_modeが設定されていません." << std::endl;
61+
return false;
62+
}
63+
64+
joint_names.push_back(joint_name);
65+
auto joint_id = config[joint_name]["id"].as<int>();
66+
auto ope_mode = config[joint_name]["operating_mode"].as<int>();
67+
double position_limit_margin = 0;
68+
double current_limit_margin = 0;
69+
if (config[joint_name]["pos_limit_margin"]) {
70+
position_limit_margin = config[joint_name]["pos_limit_margin"].as<double>();
71+
}
72+
if (config[joint_name]["current_limit_margin"]) {
73+
current_limit_margin = config[joint_name]["current_limit_margin"].as<double>();
74+
}
75+
76+
auto joint = joint::Joint(joint_id, ope_mode);
77+
joint.set_position_limit_margin(position_limit_margin);
78+
joint.set_current_limit_margin(current_limit_margin);
79+
parsed_joints.append_joint(joint_name, joint);
80+
}
81+
82+
std::vector<std::string> sync_read_targets;
83+
std::vector<std::string> sync_write_targets;
84+
if (config["joint_groups"][group_name]["sync_read"]) {
85+
sync_read_targets =
86+
config["joint_groups"][group_name]["sync_read"].as<std::vector<std::string>>();
87+
}
88+
if (config["joint_groups"][group_name]["sync_write"]) {
89+
sync_write_targets =
90+
config["joint_groups"][group_name]["sync_write"].as<std::vector<std::string>>();
91+
}
92+
93+
auto joint_group = joint::JointGroup(joint_names, sync_read_targets, sync_write_targets);
94+
95+
// sync_readとsync_writeの関係をチェック
96+
if (joint_group.sync_write_velocity_enabled() && !joint_group.sync_read_position_enabled()) {
97+
std::cerr << group_name << "グループはvelocityをsync_writeしますが, ";
98+
std::cerr << "positionをsync_readしません." << std::endl;
99+
std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
100+
return false;
101+
}
102+
if (joint_group.sync_write_current_enabled() && !joint_group.sync_read_position_enabled()) {
103+
std::cerr << group_name << "グループはcurrentをsync_writeしますが, ";
104+
std::cerr << "positionをsync_readしません." << std::endl;
105+
std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
106+
return false;
107+
}
108+
109+
parsed_joints.append_group(group_name, joint_group);
110+
}
111+
112+
return true;
113+
}
114+
115+
} // namespace config_file_parser

‎rt_manipulators_lib/src/hardware.cpp

+39-128
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "hardware.hpp"
16-
#include <yaml-cpp/yaml.h>
1715
#include <algorithm>
1816
#include <cmath>
19-
#include <fstream>
2017
#include <iostream>
2118

19+
#include "hardware.hpp"
20+
#include "config_file_parser.hpp"
21+
2222
namespace rt_manipulators_cpp {
2323

2424
// Ref: https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/
@@ -90,22 +90,48 @@ Hardware::~Hardware() {
9090

9191
bool Hardware::load_config_file(const std::string& config_yaml) {
9292
// コンフィグファイルの読み込み
93-
if (parse_config_file(config_yaml) == false) {
93+
if (config_file_parser::parse(config_yaml, joints_) == false) {
9494
return false;
9595
}
9696

97-
// sync_readとsync_writeの関係をチェック
9897
for (const auto& [group_name, group] : joints_.groups()) {
99-
if (group->sync_write_velocity_enabled() && !group->sync_read_position_enabled()) {
100-
std::cerr << group_name << "グループはvelocityをsync_writeしますが, ";
101-
std::cerr << "positionをsync_readしません." << std::endl;
102-
std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
98+
// ジョイントリミットの読み込みと設定
99+
for (const auto& joint_name : group->joint_names()) {
100+
auto joint_id = joints_.joint(joint_name)->id();
101+
uint32_t dxl_max_pos_limit = 0;
102+
uint32_t dxl_min_pos_limit = 0;
103+
uint16_t dxl_current_limit = 0;
104+
if (!comm_->read_double_word_data(joint_id, ADDR_MAX_POSITION_LIMIT, dxl_max_pos_limit)) {
105+
std::cerr << joint_name << "のMax Position Limitの読み取りに失敗しました." << std::endl;
106+
return false;
107+
}
108+
if (!comm_->read_double_word_data(joint_id, ADDR_MIN_POSITION_LIMIT, dxl_min_pos_limit)) {
109+
std::cerr << joint_name << "のMin Position Limitの読み取りに失敗しました." << std::endl;
110+
return false;
111+
}
112+
if (!comm_->read_word_data(joint_id, ADDR_CURRENT_LIMIT, dxl_current_limit)) {
113+
std::cerr << joint_name << "のCurrent Limitの読み取りに失敗しました." << std::endl;
114+
return false;
115+
}
116+
117+
joints_.joint(joint_name)->set_position_limit(
118+
dxl_pos_to_radian(static_cast<int32_t>(dxl_min_pos_limit)),
119+
dxl_pos_to_radian(static_cast<int32_t>(dxl_max_pos_limit)));
120+
joints_.joint(joint_name)->set_current_limit(
121+
dxl_current_to_ampere(dxl_current_limit));
122+
}
123+
124+
if (!write_operating_mode(group_name)) {
125+
std::cerr << group_name << "のOperating Modeを設定できません." << std::endl;
103126
return false;
104127
}
105-
if (group->sync_write_current_enabled() && !group->sync_read_position_enabled()) {
106-
std::cerr << group_name << "グループはcurrentをsync_writeしますが, ";
107-
std::cerr << "positionをsync_readしません." << std::endl;
108-
std::cerr << "positionもsync_readするようにコンフィグファイルを修正して下さい." << std::endl;
128+
129+
if (!create_sync_read_group(group_name)) {
130+
std::cerr << group_name << "のsync readグループを作成できません." << std::endl;
131+
return false;
132+
}
133+
if (!create_sync_write_group(group_name)) {
134+
std::cerr << group_name << "のsync writeグループを作成できません." << std::endl;
109135
return false;
110136
}
111137
}
@@ -734,121 +760,6 @@ bool Hardware::write_double_word_data_to_group(const std::string& group_name,
734760
return retval;
735761
}
736762

737-
bool Hardware::parse_config_file(const std::string& config_yaml) {
738-
// yamlファイルを読み取り、joints_に格納する
739-
std::ifstream fs(config_yaml);
740-
if (!fs.is_open()) {
741-
std::cerr << "コンフィグファイル:" << config_yaml << "が存在しません." << std::endl;
742-
return false;
743-
}
744-
745-
YAML::Node config = YAML::LoadFile(config_yaml);
746-
for (const auto & config_joint_group : config["joint_groups"]) {
747-
auto group_name = config_joint_group.first.as<std::string>();
748-
if (joints_.has_group(group_name)) {
749-
std::cerr << group_name << "グループが2つ以上存在します." << std::endl;
750-
return false;
751-
}
752-
753-
if (!config["joint_groups"][group_name]["joints"]) {
754-
std::cerr << group_name << "グループに'joints'が設定されていせん。" << std::endl;
755-
return false;
756-
}
757-
758-
std::vector<JointName> joint_names;
759-
for (const auto & config_joint : config["joint_groups"][group_name]["joints"]) {
760-
auto joint_name = config_joint.as<std::string>();
761-
if (joints_.has_joint(joint_name)) {
762-
std::cerr << joint_name << "ジョイントが2つ以上存在します." << std::endl;
763-
return false;
764-
}
765-
766-
if (!config[joint_name]) {
767-
std::cerr << joint_name << "ジョイントの設定が存在しません." << std::endl;
768-
return false;
769-
}
770-
771-
if (!config[joint_name]["id"] || !config[joint_name]["operating_mode"]) {
772-
std::cerr << joint_name << "にidまたはoperating_modeが設定されていません." << std::endl;
773-
return false;
774-
}
775-
776-
joint_names.push_back(joint_name);
777-
auto joint_id = config[joint_name]["id"].as<int>();
778-
auto ope_mode = config[joint_name]["operating_mode"].as<int>();
779-
780-
uint32_t dxl_max_pos_limit = 0;
781-
uint32_t dxl_min_pos_limit = 0;
782-
uint16_t dxl_current_limit = 0;
783-
if (!comm_->read_double_word_data(joint_id, ADDR_MAX_POSITION_LIMIT, dxl_max_pos_limit)) {
784-
std::cerr << joint_name << "のMax Position Limitの読み取りに失敗しました." << std::endl;
785-
return false;
786-
}
787-
if (!comm_->read_double_word_data(joint_id, ADDR_MIN_POSITION_LIMIT, dxl_min_pos_limit)) {
788-
std::cerr << joint_name << "のMin Position Limitの読み取りに失敗しました." << std::endl;
789-
return false;
790-
}
791-
if (!comm_->read_word_data(joint_id, ADDR_CURRENT_LIMIT, dxl_current_limit)) {
792-
std::cerr << joint_name << "のCurrent Limitの読み取りに失敗しました." << std::endl;
793-
return false;
794-
}
795-
796-
// 角度リミット値をラジアンに変換
797-
double max_position_limit = dxl_pos_to_radian(static_cast<int32_t>(dxl_max_pos_limit));
798-
double min_position_limit = dxl_pos_to_radian(static_cast<int32_t>(dxl_min_pos_limit));
799-
800-
// 角度リミット値にマージンを加算する
801-
if (config[joint_name]["pos_limit_margin"]) {
802-
max_position_limit -= config[joint_name]["pos_limit_margin"].as<double>();
803-
min_position_limit += config[joint_name]["pos_limit_margin"].as<double>();
804-
}
805-
806-
// 電流リミット値をアンペアに変換
807-
double current_limit = dxl_current_to_ampere(dxl_current_limit);
808-
// 電流リミット値にマージンを加算する
809-
if (config[joint_name]["current_limit_margin"]) {
810-
// current_limitは0以上の値にする
811-
current_limit = std::max(
812-
current_limit - config[joint_name]["current_limit_margin"].as<double>(),
813-
0.0);
814-
}
815-
816-
auto joint = joint::Joint(
817-
joint_id, ope_mode, max_position_limit, min_position_limit, current_limit);
818-
joints_.append_joint(joint_name, joint);
819-
}
820-
std::vector<std::string> sync_read_targets;
821-
std::vector<std::string> sync_write_targets;
822-
if (config["joint_groups"][group_name]["sync_read"]) {
823-
sync_read_targets =
824-
config["joint_groups"][group_name]["sync_read"].as<std::vector<std::string>>();
825-
}
826-
if (config["joint_groups"][group_name]["sync_write"]) {
827-
sync_write_targets =
828-
config["joint_groups"][group_name]["sync_write"].as<std::vector<std::string>>();
829-
}
830-
831-
auto joint_group = joint::JointGroup(joint_names, sync_read_targets, sync_write_targets);
832-
joints_.append_group(group_name, joint_group);
833-
834-
if (!write_operating_mode(group_name)) {
835-
std::cerr << group_name << "のOperating Modeを設定できません." << std::endl;
836-
return false;
837-
}
838-
839-
if (!create_sync_read_group(group_name)) {
840-
std::cerr << group_name << "のsync readグループを作成できません." << std::endl;
841-
return false;
842-
}
843-
if (!create_sync_write_group(group_name)) {
844-
std::cerr << group_name << "のsync writeグループを作成できません." << std::endl;
845-
return false;
846-
}
847-
}
848-
849-
return true;
850-
}
851-
852763
bool Hardware::write_operating_mode(const std::string& group_name) {
853764
// サーボモータから動作モードを読み取り、
854765
// コンフィグファイルと一致しない場合、動作モードを書き込む

0 commit comments

Comments
 (0)