diff --git a/data/action/left_kick.json b/data/action/left_kick.json deleted file mode 100644 index 1319cfa..0000000 --- a/data/action/left_kick.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Left Kick", - "next": "left_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": 25, - "left_ankle_roll": 0, - "left_elbow": 45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": 5, - "left_shoulder_roll": -20, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": 55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": -60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.003 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "kick", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/right_kick.json b/data/action/right_kick.json deleted file mode 100644 index 2f3dc86..0000000 --- a/data/action/right_kick.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Right Kick", - "next": "right_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_walkready", - "pause": 0, - "speed": 1 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "r_kick", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/sit_down.json b/data/action/sit_down.json deleted file mode 100644 index fc419d8..0000000 --- a/data/action/sit_down.json +++ /dev/null @@ -1,89 +0,0 @@ -{ - "name": "Sit Down", - "next": "right_kick", - "poses": [ - { - "joints": { - "left_ankle_pitch": -100, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "down", - "pause": 0, - "speed": 1 - }, - { - "joints": { - "left_ankle_pitch": 50, - "left_ankle_roll": 50, - "left_elbow": 50, - "left_hip_pitch": 50, - "left_hip_roll": 50, - "left_hip_yaw": 50, - "left_knee": 50, - "left_shoulder_pitch": 53, - "left_shoulder_roll": 51, - "neck_pitch": 52, - "neck_yaw": 51, - "right_ankle_pitch": 51, - "right_ankle_roll": 50, - "right_elbow": 50, - "right_hip_pitch": 50, - "right_hip_roll": 50, - "right_hip_yaw": 50, - "right_knee": 50, - "right_shoulder_pitch": 53, - "right_shoulder_roll": 51 - }, - "name": "sit_feet", - "pause": 1, - "speed": 0.01 - }, - { - "joints": { - "left_ankle_pitch": 0, - "left_ankle_roll": 0, - "left_elbow": 0, - "left_hip_pitch": 0, - "left_hip_roll": 0, - "left_hip_yaw": 0, - "left_knee": 0, - "left_shoulder_pitch": 0, - "left_shoulder_roll": 1, - "neck_pitch": 2, - "neck_yaw": 1, - "right_ankle_pitch": 1, - "right_ankle_roll": 0, - "right_elbow": 0, - "right_hip_pitch": 0, - "right_hip_roll": 0, - "right_hip_yaw": 0, - "right_knee": 0, - "right_shoulder_pitch": 0, - "right_shoulder_roll": 1 - }, - "name": "sit_up", - "pause": 0, - "speed": 0.3 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/walk_ready.json b/data/action/walk_ready.json deleted file mode 100644 index 90945ab..0000000 --- a/data/action/walk_ready.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "name": "Walk Ready", - "next": "", - "poses": [ - { - "joints": { - "left_ankle_pitch": -25, - "left_ankle_roll": 0, - "left_elbow": -45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": -5, - "left_shoulder_roll": 18, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": -55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": 60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.003 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/data/action/walkready.json b/data/action/walkready.json deleted file mode 100644 index d8f1aec..0000000 --- a/data/action/walkready.json +++ /dev/null @@ -1,35 +0,0 @@ -{ - "name": "Walkready", - "next": "", - "poses": [ - { - "joints": { - "left_ankle_pitch": -25, - "left_ankle_roll": 0, - "left_elbow": -45, - "left_hip_pitch": 55, - "left_hip_roll": -2, - "left_hip_yaw": 0, - "left_knee": -60, - "left_shoulder_pitch": -5, - "left_shoulder_roll": 20, - "neck_pitch": -5, - "neck_yaw": 1, - "right_ankle_pitch": 25, - "right_ankle_roll": 0, - "right_elbow": 45, - "right_hip_pitch": -55, - "right_hip_roll": 2, - "right_hip_yaw": 0, - "right_knee": 60, - "right_shoulder_pitch": 5, - "right_shoulder_roll": -20 - }, - "name": "walkready", - "pause": 0, - "speed": 0.0031 - } - ], - "start_delay": 0, - "stop_delay": 0 -} \ No newline at end of file diff --git a/src/action_main.cpp b/src/action_main.cpp index d09b48a..dafe790 100644 --- a/src/action_main.cpp +++ b/src/action_main.cpp @@ -35,14 +35,22 @@ using namespace std::chrono_literals; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); + + const char * help_message = + "Usage: ros2 run akushon action \n" + " Path to the action configuration file\n" + " Name of the action to run\n"; - if (argc < 2) { - std::cerr << "Please specify the path!" << std::endl; + + if (argc < 3) { + std::cerr << "Bad arguments!\n"; + std::cerr << help_message; return 0; } auto action_manager = std::make_shared(); std::string path = argv[1]; + std::string action_name = argv[2]; action_manager->load_config(path); auto node = std::make_shared("akushon_node"); @@ -51,7 +59,7 @@ int main(int argc, char * argv[]) rclcpp::Rate rcl_rate(8ms); int time = 0; - if (action_node->start(akushon::ActionName::WALKREADY)) { + if (action_node->start(action_name)) { while (rclcpp::ok()) { rcl_rate.sleep(); diff --git a/src/akushon/action/model/action_name.cpp b/src/akushon/action/model/action_name.cpp index e955fef..8548cc8 100644 --- a/src/akushon/action/model/action_name.cpp +++ b/src/akushon/action/model/action_name.cpp @@ -41,7 +41,7 @@ const char * ActionName::WALKREADY = "walk_ready"; const char * ActionName::SIT_DOWN = "sit_down"; const char * ActionName::FORWARD_UP = "forward_up"; const char * ActionName::BACKWARD_UP = "backward_up"; -const char * ActionName::LEFTWARD_UP = "leftward_ups"; +const char * ActionName::LEFTWARD_UP = "leftward_up"; const char * ActionName::RIGHTWARD_UP = "rightward_up"; const char * ActionName::RIGHT_KICK = "right_kick"; const char * ActionName::LEFT_KICK = "left_kick"; diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index 6919c3b..f58e1e9 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/src/akushon/action/node/action_manager.cpp @@ -75,8 +75,6 @@ void ActionManager::load_config(const std::string & path) name += file_name[i]; } - // remove "/" from the start of the name string - name.erase(0, 1); try { std::ifstream file(file_name); nlohmann::json action_data = nlohmann::json::parse(file); @@ -85,8 +83,9 @@ void ActionManager::load_config(const std::string & path) actions.insert({name, action}); } catch (nlohmann::json::parse_error & ex) { - // TODO(any): will be used for logging - // std::cerr << "parse error at byte " << ex.byte << std::endl; + std::cerr << "failed to load action: " << name << std::endl; + std::cerr << "parse error at byte " << ex.byte << std::endl; + throw ex; } } } @@ -97,41 +96,36 @@ Action ActionManager::load_action( { Action action = Action(action_name); - try { - action.set_name(action_data["name"]); + action.set_name(action_data["name"]); - for (const auto & [key, val] : action_data.items()) { - if (key.find("poses") != std::string::npos) { - for (const auto & raw_pose : action_data["poses"]) { - { - using tachimawari::joint::JointId; - using tachimawari::joint::Joint; + for (const auto & [key, val] : action_data.items()) { + if (key.find("poses") != std::string::npos) { + for (const auto & raw_pose : action_data["poses"]) { + { + using tachimawari::joint::JointId; + using tachimawari::joint::Joint; - Pose pose(raw_pose["name"]); - std::vector joints; + Pose pose(raw_pose["name"]); + std::vector joints; - for (const auto & [joint_key, joint_val] : raw_pose["joints"].items()) { - Joint joint(JointId::by_name.at(joint_key), joint_val); - joints.push_back(joint); - } - - pose.set_pause(raw_pose["pause"]); - pose.set_speed(raw_pose["speed"]); - pose.set_joints(joints); - action.add_pose(pose); + for (const auto & [joint_key, joint_val] : raw_pose["joints"].items()) { + Joint joint(JointId::by_name.at(joint_key), joint_val); + joints.push_back(joint); } + + pose.set_pause(raw_pose["pause"]); + pose.set_speed(raw_pose["speed"]); + pose.set_joints(joints); + action.add_pose(pose); } - } else if (key == "start_delay") { - action.set_start_delay(val); - } else if (key == "stop_delay") { - action.set_stop_delay(val); - } else if (key == "next") { - action.set_next_action(val); } + } else if (key == "start_delay") { + action.set_start_delay(val); + } else if (key == "stop_delay") { + action.set_stop_delay(val); + } else if (key == "next") { + action.set_next_action(val); } - } catch (nlohmann::json::parse_error & ex) { - // TODO(any): will be used for logging - // std::cerr << "parse error at byte " << ex.byte << std::endl; } return action; diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index 6a93872..2100afa 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/src/akushon/action/node/action_node.cpp @@ -73,14 +73,15 @@ ActionNode::ActionNode( run_action_subscriber = node->create_subscription( run_action_topic(), 10, [this](std::shared_ptr message) { - std::cout << message->action_name << std::endl; - if (message->control_type == RUN_ACTION_BY_NAME) { - this->start(message->action_name); - } else { - nlohmann::json action_data = nlohmann::json::parse(message->json); - Action action = this->action_manager->load_action(action_data, message->action_name); - - this->start(action); + if (!this->action_manager->is_playing()) { + if (message->control_type == RUN_ACTION_BY_NAME) { + this->start(message->action_name); + } else { + nlohmann::json action_data = nlohmann::json::parse(message->json); + Action action = this->action_manager->load_action(action_data, message->action_name); + + this->start(action); + } } }); @@ -142,6 +143,8 @@ void ActionNode::publish_joints() joint_msgs[i].position = joints[i].get_position(); } + joints_msg.control_type = tachimawari::joint::Middleware::FOR_ACTION; + set_joints_publisher->publish(joints_msg); } diff --git a/src/akushon/action/process/interpolator.cpp b/src/akushon/action/process/interpolator.cpp index 7822e91..3c9acea 100644 --- a/src/akushon/action/process/interpolator.cpp +++ b/src/akushon/action/process/interpolator.cpp @@ -56,7 +56,7 @@ void Interpolator::process(int time) start_stop_time = time; } - if ((time - start_stop_time) > (get_current_action().get_start_delay() * 1000)) { + if ((time - start_stop_time) > (get_current_action().get_start_delay())) { change_state(PLAYING); } @@ -74,7 +74,7 @@ void Interpolator::process(int time) if (current_pose_index == get_current_action().get_pose_count()) { change_state(STOP_DELAY); init_pause = true; - } else if ((time - pause_time) > (get_current_pose().get_pause() * 1000)) { + } else if ((time - pause_time) > (get_current_pose().get_pause())) { next_pose(); init_pause = true; } @@ -90,12 +90,13 @@ void Interpolator::process(int time) start_stop_time = time; } - if ((time - start_stop_time) > (get_current_action().get_stop_delay() * 1000)) { + if ((time - start_stop_time) > (get_current_action().get_stop_delay())) { ++current_action_index; if (current_action_index == actions.size()) { change_state(END); } else { + current_pose_index = 0; change_state(START_DELAY); } } diff --git a/src/akushon/node/akushon_node.cpp b/src/akushon/node/akushon_node.cpp index 0db0315..c23af71 100644 --- a/src/akushon/node/akushon_node.cpp +++ b/src/akushon/node/akushon_node.cpp @@ -44,7 +44,7 @@ AkushonNode::AkushonNode(rclcpp::Node::SharedPtr node) [this]() { if (this->action_node) { double time = this->node->now().seconds() - this->start_time; - this->action_node->update(time * 1000); + this->action_node->update(time); } } );