32 static inline std::string &
ltrim(std::string &s)
34 s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
37 static inline std::string &
rtrim(std::string &s)
39 s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
42 static inline std::string &
trim(std::string &s)
47 static inline std::vector<std::string>
split(
const std::string &text,
char sep)
49 std::vector<std::string> tokens;
50 std::size_t
start = 0, end = 0;
52 while ((end = text.find(sep, start)) != (std::string::npos))
54 tokens.push_back(text.substr(start, end - start));
58 tokens.push_back(text.substr(start));
64 Robot::Robot(std::string robot_file_path, std::string dev_desc_dir_path)
67 if (dev_desc_dir_path.compare(dev_desc_dir_path.size() - 1, 1,
"/") != 0)
68 dev_desc_dir_path +=
"/";
70 std::ifstream file(robot_file_path.c_str());
73 std::string session =
"";
74 std::string input_str;
77 std::getline(file, input_str);
80 std::size_t pos = input_str.find(
"#");
81 if (pos != std::string::npos)
82 input_str = input_str.substr(0, pos);
85 input_str =
trim(input_str);
88 if (!input_str.compare(0, 1,
"[") && !input_str.compare(input_str.size() - 1, 1,
"]"))
90 input_str = input_str.substr(1, input_str.size() - 2);
91 std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
92 session =
trim(input_str);
98 std::vector<std::string> tokens =
split(input_str,
'=');
99 if (tokens.size() != 2)
102 if (tokens[0] ==
"control_cycle")
107 std::vector<std::string> tokens =
split(input_str,
'|');
108 if (tokens.size() != 3)
111 std::cout << tokens[0] <<
" added. (baudrate: " << tokens[1] <<
")" << std::endl;
114 ports_[tokens[0]]->setBaudRate(std::atoi(tokens[1].c_str()));
119 std::vector<std::string> tokens =
split(input_str,
'|');
120 if (tokens.size() != 7)
125 std::string file_path = dev_desc_dir_path + tokens[0] +
"/" + tokens[3] +
".device";
126 int id = std::atoi(tokens[2].c_str());
127 std::string port = tokens[1];
128 float protocol = std::atof(tokens[4].c_str());
129 std::string dev_name = tokens[5];
134 std::vector<std::string> sub_tokens =
split(tokens[6],
',');
135 if (sub_tokens.size() > 0)
141 for (
int _i = 0; _i < sub_tokens.size(); _i++)
143 std::map<std::string, ControlTableItem *>::iterator bulkread_it = dxl->
ctrl_table_.find(sub_tokens[_i]);
146 fprintf(stderr,
"\n ##### BULK READ ITEM [ %s ] NOT FOUND!! #####\n\n", sub_tokens[_i].c_str());
155 dest_item->
address_ = indirect_data_addr;
168 for (
int i = 0; i < sub_tokens.size(); i++)
176 else if (tokens[0] ==
SENSOR)
178 std::string file_path = dev_desc_dir_path + tokens[0] +
"/" + tokens[3] +
".device";
179 int id = std::atoi(tokens[2].c_str());
180 std::string port = tokens[1];
181 float protocol = std::atof(tokens[4].c_str());
182 std::string dev_name = tokens[5];
187 std::vector<std::string> sub_tokens =
split(tokens[6],
',');
188 if (sub_tokens.size() > 0)
194 for (
int i = 0; i < sub_tokens.size(); i++)
201 dest_item->
address_ = indirect_data_addr;
214 for (
int i = 0; i < sub_tokens.size(); i++)
225 std::cout <<
"Unable to open file : " + robot_file_path << std::endl;
234 std::ifstream file(path.c_str());
237 std::string session =
"";
238 std::string input_str;
242 std::getline(file, input_str);
245 std::size_t pos = input_str.find(
"#");
246 if (pos != std::string::npos)
247 input_str = input_str.substr(0, pos);
250 input_str =
trim(input_str);
255 if (!input_str.compare(0, 1,
"[") && !input_str.compare(input_str.size() - 1, 1,
"]"))
257 input_str = input_str.substr(1, input_str.size() - 2);
258 std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
259 session =
trim(input_str);
265 std::vector<std::string> tokens =
split(input_str,
'=');
266 if (tokens.size() != 2)
269 if (tokens[0] ==
"model_name")
270 sensor =
new Sensor(
id, tokens[1], protocol_version);
274 std::vector<std::string> tokens =
split(input_str,
'|');
275 if (tokens.size() != 8)
280 item->
address_ = std::atoi(tokens[0].c_str());
283 if (tokens[3] ==
"R")
285 else if (tokens[3] ==
"RW")
288 if (tokens[4] ==
"EEPROM")
290 else if (tokens[4] ==
"RAM")
296 if (tokens[7] ==
"Y")
298 else if (tokens[7] ==
"N")
305 fprintf(stderr,
"(%s) [ID:%3d] %14s added. \n", port.c_str(), sensor->
id_, sensor->
model_name_.c_str());
310 std::cout <<
"Unable to open file : " + path << std::endl;
320 std::ifstream file(path.c_str());
323 std::string session =
"";
324 std::string input_str;
326 std::string torque_enable_item_name =
"";
327 std::string present_position_item_name =
"";
328 std::string present_velocity_item_name =
"";
329 std::string present_current_item_name =
"";
330 std::string goal_position_item_name =
"";
331 std::string goal_velocity_item_name =
"";
332 std::string goal_current_item_name =
"";
333 std::string position_d_gain_item_name =
"";
334 std::string position_i_gain_item_name =
"";
335 std::string position_p_gain_item_name =
"";
336 std::string velocity_d_gain_item_name =
"";
337 std::string velocity_i_gain_item_name =
"";
338 std::string velocity_p_gain_item_name =
"";
342 std::getline(file, input_str);
345 std::size_t pos = input_str.find(
"#");
346 if (pos != std::string::npos)
347 input_str = input_str.substr(0, pos);
350 input_str =
trim(input_str);
355 if (!input_str.compare(0, 1,
"[") && !input_str.compare(input_str.size() - 1, 1,
"]"))
357 input_str = input_str.substr(1, input_str.size() - 2);
358 std::transform(input_str.begin(), input_str.end(), input_str.begin(), ::tolower);
359 session =
trim(input_str);
365 std::vector<std::string> tokens =
split(input_str,
'=');
366 if (tokens.size() != 2)
369 if (tokens[0] ==
"model_name")
370 dxl =
new Dynamixel(
id, tokens[1], protocol_version);
374 std::vector<std::string> tokens =
split(input_str,
'=');
375 if (tokens.size() != 2)
378 if (tokens[0] ==
"torque_to_current_value_ratio")
380 else if (tokens[0] ==
"velocity_to_value_ratio")
383 else if (tokens[0] ==
"value_of_0_radian_position")
385 else if (tokens[0] ==
"value_of_min_radian_position")
387 else if (tokens[0] ==
"value_of_max_radian_position")
389 else if (tokens[0] ==
"min_radian")
391 else if (tokens[0] ==
"max_radian")
394 else if (tokens[0] ==
"torque_enable_item_name")
395 torque_enable_item_name = tokens[1];
396 else if (tokens[0] ==
"present_position_item_name")
397 present_position_item_name = tokens[1];
398 else if (tokens[0] ==
"present_velocity_item_name")
399 present_velocity_item_name = tokens[1];
400 else if (tokens[0] ==
"present_current_item_name")
401 present_current_item_name = tokens[1];
402 else if (tokens[0] ==
"goal_position_item_name")
403 goal_position_item_name = tokens[1];
404 else if (tokens[0] ==
"goal_velocity_item_name")
405 goal_velocity_item_name = tokens[1];
406 else if (tokens[0] ==
"goal_current_item_name")
407 goal_current_item_name = tokens[1];
408 else if (tokens[0] ==
"position_d_gain_item_name")
409 position_d_gain_item_name = tokens[1];
410 else if (tokens[0] ==
"position_i_gain_item_name")
411 position_i_gain_item_name = tokens[1];
412 else if (tokens[0] ==
"position_p_gain_item_name")
413 position_p_gain_item_name = tokens[1];
414 else if (tokens[0] ==
"velocity_d_gain_item_name")
415 velocity_d_gain_item_name = tokens[1];
416 else if (tokens[0] ==
"velocity_i_gain_item_name")
417 velocity_i_gain_item_name = tokens[1];
418 else if (tokens[0] ==
"velocity_p_gain_item_name")
419 velocity_p_gain_item_name = tokens[1];
423 std::vector<std::string> tokens =
split(input_str,
'|');
424 if (tokens.size() != 8)
429 item->
address_ = std::atoi(tokens[0].c_str());
432 if (tokens[3] ==
"R")
434 else if (tokens[3] ==
"RW")
437 if (tokens[4] ==
"EEPROM")
439 else if (tokens[4] ==
"RAM")
445 if (tokens[7] ==
"Y")
447 else if (tokens[7] ==
"N")
454 if (dxl->
ctrl_table_[torque_enable_item_name] != NULL)
456 if (dxl->
ctrl_table_[present_position_item_name] != NULL)
458 if (dxl->
ctrl_table_[present_velocity_item_name] != NULL)
460 if (dxl->
ctrl_table_[present_current_item_name] != NULL)
462 if (dxl->
ctrl_table_[goal_position_item_name] != NULL)
464 if (dxl->
ctrl_table_[goal_velocity_item_name] != NULL)
466 if (dxl->
ctrl_table_[goal_current_item_name] != NULL)
468 if (dxl->
ctrl_table_[position_d_gain_item_name] != NULL)
470 if (dxl->
ctrl_table_[position_i_gain_item_name] != NULL)
472 if (dxl->
ctrl_table_[position_p_gain_item_name] != NULL)
474 if (dxl->
ctrl_table_[velocity_d_gain_item_name] != NULL)
476 if (dxl->
ctrl_table_[velocity_i_gain_item_name] != NULL)
478 if (dxl->
ctrl_table_[velocity_p_gain_item_name] != NULL)
481 fprintf(stderr,
"(%s) [ID:%3d] %14s added. \n", port.c_str(), dxl->
id_, dxl->
model_name_.c_str());
486 std::cout <<
"Unable to open file : " + path << std::endl;
#define SESSION_CONTROL_INFO
static PortHandler * getPortHandler(const char *port_name)
int32_t value_of_0_radian_position_
Dynamixel * getDynamixel(std::string path, int id, std::string port, float protocol_version)
int32_t value_of_max_radian_position_
#define DEFAULT_CONTROL_CYCLE
ControlTableItem * present_velocity_item_
ControlTableItem * position_p_gain_item_
static std::string & rtrim(std::string &s)
ControlTableItem * velocity_p_gain_item_
#define SESSION_PORT_INFO
Robot(std::string robot_file_path, std::string dev_desc_dir_path)
std::map< std::string, dynamixel::PortHandler * > ports_
std::vector< ControlTableItem * > bulk_read_items_
std::map< std::string, Dynamixel * > dxls_
ControlTableItem * present_position_item_
std::map< std::string, std::string > port_default_device_
ControlTableItem * goal_current_item_
double torque_to_current_value_ratio_
#define SESSION_CONTROL_TABLE
#define SESSION_TYPE_INFO
ControlTableItem * goal_position_item_
ControlTableItem * torque_enable_item_
ControlTableItem * position_i_gain_item_
ControlTableItem * velocity_i_gain_item_
ControlTableItem * position_d_gain_item_
std::map< std::string, Sensor * > sensors_
int32_t value_of_min_radian_position_
ControlTableItem * goal_velocity_item_
static std::vector< std::string > split(const std::string &text, char sep)
static std::string & ltrim(std::string &s)
#define SESSION_DEVICE_INFO
ControlTableItem * velocity_d_gain_item_
double velocity_to_value_ratio_
Sensor * getSensor(std::string path, int id, std::string port, float protocol_version)
#define INDIRECT_ADDRESS_1
ControlTableItem * present_current_item_
static std::string & trim(std::string &s)
std::map< std::string, ControlTableItem * > ctrl_table_