From 32cb2a9dfceedf772b1b5e5680b940ea37a32776 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 4 Oct 2019 17:37:38 +0900 Subject: [PATCH 1/2] fixed some bugs include jumping before start to play action. --- op3_action_editor/CMakeLists.txt | 4 + .../include/op3_action_editor/action_editor.h | 8 +- op3_action_editor/package.xml | 1 + op3_action_editor/src/action_editor.cpp | 115 ++++++++++++++++-- op3_action_editor/src/main.cpp | 17 ++- op3_camera_setting_tool/cfg/CameraParams.cfg | 0 op3_gui_demo/src/qnode_default_demo.cpp | 0 7 files changed, 127 insertions(+), 18 deletions(-) mode change 100755 => 100644 op3_camera_setting_tool/cfg/CameraParams.cfg mode change 100755 => 100644 op3_gui_demo/src/qnode_default_demo.cpp diff --git a/op3_action_editor/CMakeLists.txt b/op3_action_editor/CMakeLists.txt index 6da0582..557e619 100644 --- a/op3_action_editor/CMakeLists.txt +++ b/op3_action_editor/CMakeLists.txt @@ -3,6 +3,8 @@ ################################################################################ cmake_minimum_required(VERSION 2.8.3) project(op3_action_editor) +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) ################################################################################ # Find catkin packages and libraries for catkin and system dependencies @@ -14,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS robotis_controller robotis_device op3_action_module + op3_base_module ) find_package(Curses REQUIRED) @@ -61,6 +64,7 @@ catkin_package( robotis_controller robotis_device op3_action_module + op3_base_module DEPENDS CURSES ) diff --git a/op3_action_editor/include/op3_action_editor/action_editor.h b/op3_action_editor/include/op3_action_editor/action_editor.h index 16ff952..8ba9c02 100644 --- a/op3_action_editor/include/op3_action_editor/action_editor.h +++ b/op3_action_editor/include/op3_action_editor/action_editor.h @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2017 ROBOTIS CO., LTD. +* Copyright 2019 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ * limitations under the License. *******************************************************************************/ -/* Author: Jay Song */ +/* Author: Jay Song, Kayman Jung */ #ifndef OP3_ACTION_EDITOR_H_ #define OP3_ACTION_EDITOR_H_ @@ -35,6 +35,7 @@ #include "dynamixel_sdk/dynamixel_sdk.h" #include "robotis_controller/robotis_controller.h" #include "op3_action_module/action_module.h" +#include "op3_base_module/base_module.h" #define ROBOT_NAME "OP3" @@ -117,6 +118,7 @@ class ActionEditor void copyCmd(int index); void newCmd(); void goCmd(int index); + void goCmd_2(int index); void saveCmd(); void nameCmd(); @@ -128,7 +130,9 @@ class ActionEditor void resetSTDin(); int convert4095ToPositionValue(int id, int w4095); + double convert4095ToRadPosition(int id, int w4095); int convertPositionValueTo4095(int id, int PositionValue); + double convertPositionValueToRad(int id, int PositionValue); int convert4095ToMirror(int id, int w4095); bool loadMp3Path(int mp3_index, std::string &path); diff --git a/op3_action_editor/package.xml b/op3_action_editor/package.xml index 95888c0..897ead6 100644 --- a/op3_action_editor/package.xml +++ b/op3_action_editor/package.xml @@ -20,6 +20,7 @@ robotis_controller robotis_device op3_action_module + op3_base_module libncurses-dev yaml-cpp op3_manager diff --git a/op3_action_editor/src/action_editor.cpp b/op3_action_editor/src/action_editor.cpp index f160894..e090ff1 100644 --- a/op3_action_editor/src/action_editor.cpp +++ b/op3_action_editor/src/action_editor.cpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2017 ROBOTIS CO., LTD. +* Copyright 2019 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ * limitations under the License. *******************************************************************************/ -/* Author: Jay Song */ +/* Author: Jay Song, Kayman Jung */ #include "op3_action_editor/action_editor.h" @@ -329,8 +329,8 @@ bool ActionEditor::initializeActionEditor(std::string robot_file_path, std::stri 8); } - default_editor_script_path_ = ros::package::getPath("op3_action_editor") + "/config/editor_script.yaml"; - mirror_joint_file_path_ = ros::package::getPath("op3_action_editor") + "/config/config_mirror_joint.yaml"; + default_editor_script_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + "/script/editor_script.yaml"; + mirror_joint_file_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + "/config/config_mirror_joint.yaml"; // for mirroring upper_body_mirror_joints_rl_.clear(); @@ -349,10 +349,22 @@ int ActionEditor::convert4095ToPositionValue(int id, int w4095) return robot_->dxls_[joint_id_to_name_[id]]->convertRadian2Value(rad); } +double ActionEditor::convert4095ToRadPosition(int id, int w4095) +{ + double rad = (w4095 - 2048)*M_PI/2048.0; + return rad; +} + int ActionEditor::convertPositionValueTo4095(int id, int PositionValue) { double rad = robot_->dxls_[joint_id_to_name_[id]]->convertValue2Radian(PositionValue); - return (int) ((rad + M_PI) * 2048.0 / M_PI); + return (int)((rad + M_PI)*2048.0/M_PI); +} + +double ActionEditor::convertPositionValueToRad(int id, int PositionValue) +{ + double rad = robot_->dxls_[joint_id_to_name_[id]]->convertValue2Radian(PositionValue); + return rad; } int ActionEditor::convert4095ToMirror(int id, int w4095) @@ -856,6 +868,9 @@ void ActionEditor::setValue(int value) printf("%.4d", value); edited_ = true; } + + double rad_position = convert4095ToRadPosition(id, value); + ctrl_->robot_->dxls_[joint_id_to_name_[id]]->dxl_state_->goal_position_ = rad_position; } } } @@ -1090,8 +1105,12 @@ void ActionEditor::toggleTorque() - robot_->dxls_[joint_name]->value_of_0_radian_position_; value = value - offset; - step_.position[id] = convertPositionValueTo4095(id, value); - printf("%.4d", value); + int w4096_value = convertPositionValueTo4095(id, value); + step_.position[id] = w4096_value; + printf("%.4d", w4096_value); + + // set goal position + robot_->dxls_[joint_name]->dxl_state_->goal_position_ = robot_->dxls_[joint_name]->convertValue2Radian(value); } else { @@ -1262,14 +1281,14 @@ void ActionEditor::playCmd(int mp3_index) ctrl_->startTimer(); ros::Duration(0.03).sleep(); // waiting for timer start - std_msgs::String msg; - msg.data = "action_module"; - enable_ctrl_module_pub_.publish(msg); - ros::Duration(0.03).sleep(); // waiting for enable + std::string module_name = "action_module"; + ctrl_->setCtrlModule(module_name); + ros::Duration(0.03).sleep(); // waiting for enable if (ActionModule::getInstance()->start(page_idx_, &page_) == false) { printCmd("Failed to play this page!"); + ctrl_->setCtrlModule("none"); ctrl_->stopTimer(); return; } @@ -1317,6 +1336,7 @@ void ActionEditor::playCmd(int mp3_index) } resetSTDin(); + ctrl_->setCtrlModule("none"); ctrl_->stopTimer(); goToCursor(cmd_col_, cmd_row_); @@ -1798,6 +1818,9 @@ void ActionEditor::goCmd(int index) { it->second->addParam(id, param); } + + // set goal position + robot_->dxls_[joint_name]->dxl_state_->goal_position_ = convert4095ToRadPosition(id, page_.step[index].position[id]); } for (std::map::iterator it = port_to_sync_write_go_cmd_.begin(); @@ -1821,7 +1844,7 @@ void ActionEditor::goCmd(int index) distance = 0; - goal_position = page_.step[index].position[id]; + goal_position = convert4095ToPositionValue(id, page_.step[index].position[id]); int offset = robot_->dxls_[joint_name]->convertRadian2Value(robot_->dxls_[joint_name]->dxl_state_->position_offset_) - robot_->dxls_[joint_name]->value_of_0_radian_position_; @@ -1842,7 +1865,11 @@ void ActionEditor::goCmd(int index) it != port_to_sync_write_go_cmd_.end(); it++) { it->second->addParam(id, param); - } + } + + // set goal position + robot_->dxls_[joint_name]->dxl_state_->goal_position_ = convert4095ToRadPosition(id, page_.step[index].position[id]); + } for (std::map::iterator it = port_to_sync_write_go_cmd_.begin(); @@ -1857,6 +1884,68 @@ void ActionEditor::goCmd(int index) printf("Go Command Completed"); } +void ActionEditor::goCmd_2(int index) +{ + if (index < 0 || index >= action_file_define::MAXNUM_STEP) + { + printCmd("Invalid step index"); + return; + } + + if (index >= page_.header.stepnum) + { + printCmd("Are you sure? (y/n)"); + if (_getch() != 'y') + { + clearCmd(); + return; + } + } + + ctrl_->startTimer(); + ros::Duration(0.03).sleep(); // waiting for timer start + + BaseModule *base_module = BaseModule::getInstance(); + + //make map, key : joint_name, value : joint_init_pos_rad; + std::map go_pose; + + for (std::map::iterator it = joint_id_to_row_index_.begin(); + it != joint_id_to_row_index_.end(); + it++) + { + int id = it->first; + std::string joint_name = joint_id_to_name_[id]; + if (page_.step[index].position[id] & action_file_define::INVALID_BIT_MASK) + { + printCmd("Exist invalid joint value"); + return; + } + + double goal_position = convert4095ToRadPosition(id, page_.step[index].position[id]); + go_pose[joint_name] = goal_position; + } + + // move time : 5.0 sec + base_module->poseGenerateProc(go_pose); + + ros::Duration(0.01).sleep(); + + while (base_module->isRunning()) + ros::Duration(0.01).sleep(); + + ctrl_->stopTimer(); + while (ctrl_->isTimerRunning()) + ros::Duration(0.01).sleep(); + + ctrl_->setCtrlModule("none"); + + step_ = page_.step[index]; + drawStep(7); + goToCursor(cmd_col_, cmd_row_); + printf("Go Command Completed"); +} + void ActionEditor::saveCmd() { if (edited_ == false) diff --git a/op3_action_editor/src/main.cpp b/op3_action_editor/src/main.cpp index 9bb53c5..e18b594 100644 --- a/op3_action_editor/src/main.cpp +++ b/op3_action_editor/src/main.cpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2017 ROBOTIS CO., LTD. +* Copyright 201 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ * limitations under the License. *******************************************************************************/ -/* Author: JaySong */ +/* Author: JaySong, Kayman Jung */ #include "op3_action_editor/action_editor.h" @@ -31,6 +31,7 @@ void sighandler(int sig) term.c_lflag |= ICANON | ECHO; tcsetattr( STDIN_FILENO, TCSANOW, &term); + system("clear"); exit(0); } @@ -92,6 +93,9 @@ int main(int argc, char **argv) return 0; } + // disable ros log + ros::console::shutdown(); + editor.drawIntro(); while (1) @@ -308,6 +312,13 @@ int main(int argc, char **argv) else editor.printCmd("Need parameter"); } + else if (strcmp(cmd, "m") == 0) + { + if (num_param > 1) + editor.moveStepCmd(iparam[0], iparam[1]); + else + editor.printCmd("Need parameter"); + } else if (strcmp(cmd, "i") == 0) { if (num_param == 0) @@ -334,7 +345,7 @@ int main(int argc, char **argv) else if (strcmp(cmd, "g") == 0) { if (num_param > 0) - editor.goCmd(iparam[0]); + editor.goCmd_2(iparam[0]); else editor.printCmd("Need parameter"); } diff --git a/op3_camera_setting_tool/cfg/CameraParams.cfg b/op3_camera_setting_tool/cfg/CameraParams.cfg old mode 100755 new mode 100644 diff --git a/op3_gui_demo/src/qnode_default_demo.cpp b/op3_gui_demo/src/qnode_default_demo.cpp old mode 100755 new mode 100644 From 1f11bca02a49161974063affad55d68b88f73167 Mon Sep 17 00:00:00 2001 From: Kayman Date: Mon, 7 Oct 2019 10:38:05 +0900 Subject: [PATCH 2/2] Changing file permissions --- op3_camera_setting_tool/cfg/CameraParams.cfg | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 op3_camera_setting_tool/cfg/CameraParams.cfg diff --git a/op3_camera_setting_tool/cfg/CameraParams.cfg b/op3_camera_setting_tool/cfg/CameraParams.cfg old mode 100644 new mode 100755