From 31f7a26958b05a9b1913f90c6aa2a48c45a3e2cc Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 8 Jan 2025 17:52:27 +0100 Subject: [PATCH] Changed dynup behaviour so that dynup doesn't interfere with itself anymore. --- .vscode/settings.json | 3 ++- .../include/bitbots_dynup/dynup_node.hpp | 1 + bitbots_motion/bitbots_dynup/src/dynup_node.cpp | 16 ++++++++++++++++ .../hcm_dsd/actions/play_animation.py | 4 ++++ bitbots_motion/bitbots_hcm/src/hcm.cpp | 15 +++++++++------ bitbots_msgs/action/Dynup.action | 3 +++ bitbots_msgs/msg/JointCommand.msg | 4 ++++ 7 files changed, 39 insertions(+), 7 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 707c2f422..be0dde9f6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -214,7 +214,8 @@ "regex": "cpp", "future": "cpp", "*.ipp": "cpp", - "span": "cpp" + "span": "cpp", + "ranges": "cpp" }, // Tell the ROS extension where to find the setup.bash // This also utilizes the COLCON_WS environment variable, which needs to be set diff --git a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp index bb563e7f9..9111452a8 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp @@ -116,6 +116,7 @@ class DynupNode { double start_time_ = 0; bool server_free_ = true; bool debug_ = false; + bool cancel_goal_ = false; // TF2 related things tf2_ros::Buffer tf_buffer_; diff --git a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp index 69ed264c8..f05953faf 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp @@ -215,6 +215,10 @@ rclcpp_action::GoalResponse DynupNode::goalCb(const rclcpp_action::GoalUUID &uui server_free_ = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } else { + if (goal->from_hcm) { + cancel_goal_ = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } RCLCPP_WARN(node_->get_logger(), "Dynup is busy, goal rejected!"); return rclcpp_action::GoalResponse::REJECT; } @@ -259,11 +263,20 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr goal_ /* Do the loop as long as nothing cancels it */ while (rclcpp::ok()) { rclcpp::Time startTime = node_->get_clock()->now(); + // This is used when the cancelation is requested externally if (goal_handle->is_canceling()) { goal_handle->canceled(result); RCLCPP_INFO(node_->get_logger(), "Goal canceled"); return; } + // This is used when the dynup server descides to cancel itself + if (cancel_goal_) { + result->successful = false; + goal_handle->abort(result); + server_free_ = true; + cancel_goal_ = false; + return; + } msg = step(getTimeDelta()); auto feedback = std::make_shared(); feedback->percent_done = engine_.getPercentDone(); @@ -282,9 +295,12 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr goal_ if (msg.joint_names.empty()) { continue; } + msg.from_hcm = goal_handle->get_goal()->from_hcm; joint_goal_publisher_->publish(msg); node_->get_clock()->sleep_until(startTime + rclcpp::Duration::from_nanoseconds(1e9 / loop_rate)); } + // Wether we canceled the goal or we finished cleanly the cancelation request is not valid anymore + cancel_goal_ = false; } bitbots_dynup::msg::DynupPoses DynupNode::getCurrentPoses() { diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py index 8f805b1da..5beebe5fe 100644 --- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py +++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py @@ -231,6 +231,10 @@ def start_animation(self): goal = Dynup.Goal() goal.direction = self.direction + # This indicates that the goal is send from the hcm and therfore has prio, + # canceling other tasks. The published joint goals are also marked with this flag so they + # are handled differently in the HCM joint mutex + goal.from_hcm = True self.blackboard.dynup_action_current_goal = self.blackboard.dynup_action_client.send_goal_async( goal, feedback_callback=self.animation_feedback_cb ) diff --git a/bitbots_motion/bitbots_hcm/src/hcm.cpp b/bitbots_motion/bitbots_hcm/src/hcm.cpp index d22a44cee..e27c66268 100644 --- a/bitbots_motion/bitbots_hcm/src/hcm.cpp +++ b/bitbots_motion/bitbots_hcm/src/hcm.cpp @@ -87,12 +87,15 @@ class HCM_CPP : public rclcpp::Node { } void dynup_callback(const bitbots_msgs::msg::JointCommand msg) { - if (current_state_ == bitbots_msgs::msg::RobotControlState::STARTUP || - current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP || - current_state_ == bitbots_msgs::msg::RobotControlState::MOTOR_OFF || - current_state_ == bitbots_msgs::msg::RobotControlState::PICKED_UP || - current_state_ == bitbots_msgs::msg::RobotControlState::PENALTY || - current_state_ == bitbots_msgs::msg::RobotControlState::RECORD || + if (msg.from_hcm and (current_state_ == bitbots_msgs::msg::RobotControlState::STARTUP || + current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::MOTOR_OFF || + current_state_ == bitbots_msgs::msg::RobotControlState::PICKED_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::PENALTY || + current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE)) { + pub_controller_command_->publish(msg); + } + if (current_state_ == bitbots_msgs::msg::RobotControlState::RECORD || current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE) { pub_controller_command_->publish(msg); } diff --git a/bitbots_msgs/action/Dynup.action b/bitbots_msgs/action/Dynup.action index 6e5620f2f..939261314 100644 --- a/bitbots_msgs/action/Dynup.action +++ b/bitbots_msgs/action/Dynup.action @@ -9,6 +9,9 @@ string DIRECTION_RISE = "rise" string DIRECTION_DESCEND = "descend" string DIRECTION_WALKREADY = "walkready" +# Requests from the HCM have higher prio and are published to a different topic +bool from_hcm + --- # Result definition bool successful diff --git a/bitbots_msgs/msg/JointCommand.msg b/bitbots_msgs/msg/JointCommand.msg index 115e0db78..c86b230dc 100644 --- a/bitbots_msgs/msg/JointCommand.msg +++ b/bitbots_msgs/msg/JointCommand.msg @@ -1,4 +1,8 @@ std_msgs/Header header + +# Mark this message so it has prio +bool from_hcm + string[] joint_names float64[] positions