Changed dynup behaviour so that dynup doesn't interfere with itself a…

…nymore.

.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

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_;

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<DynupGoalHandle> 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<bitbots_msgs::action::Dynup_Feedback>();
     feedback->percent_done = engine_.getPercentDone();
@@ -282,9 +295,12 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr<DynupGoalHandle> 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() {

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
         )

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);
     }

bitbots_msgs/action/Dynup.action

@@ -11,6 +11,9 @@ string DIRECTION_WALKREADY = "walkready"
 string DIRECTION_RISE_NO_ARMS = "rise_no_arms"
 string DIRECTION_DESCEND_NO_ARMS = "descend_no_arms"
 
+# Requests from the HCM have higher prio and are published to a different topic
+bool from_hcm 
+
 ---
 # Result definition
 bool successful

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