add twist_cmd skill with condition

rbs_skill_interfaces/CMakeLists.txt

@@ -21,6 +21,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendJointStates.action"
     "action/GripperCommand.action"
     "action/MoveitSendPoseWithConstraints.action"
+    "action/TwistControlWithCondition.action"
     "action/RbsBt.action"
     "msg/ObjectInfo.msg"
     "msg/PropertyValuePair.msg"

rbs_skill_interfaces/action/TwistControlWithCondition.action

@@ -0,0 +1,8 @@
+geometry_msgs/Twist twist_cmd
+string robot_name
+---
+bool success
+bool condition_status
+---
+bool success
+bool condition_status

rbs_skill_servers/CMakeLists.txt

@@ -106,6 +106,12 @@ add_ros2_component(move_to_pose_orientation_constraint
   mtp_moveit_orientation_constraint
 )
 
+add_ros2_component(twist_command_with_condition
+  ./src/twist_cmd_with_condition.cpp
+  "rbs_skill_actions::TwistCmdWithCondition"
+  twist_cmd_with_condition
+)
+
 # Install directories and targets
 install(DIRECTORY include/ DESTINATION include)
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@@ -120,6 +126,7 @@ install(
           move_to_pose_moveit_cartesian
           move_to_pose_moveit
           move_to_pose_orientation_constraint
+          twist_command_with_condition
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})

rbs_skill_servers/launch/skills.launch.py

@@ -118,6 +118,20 @@ def launch_setup(context, *args, **kwargs):
                 ],
                 condition=IfCondition(use_moveit),
             ),
+            ComposableNode(
+                package="rbs_skill_servers",
+                plugin="rbs_skill_actions::TwistCmdWithCondition",
+                name="twist_cmd_with_condition",
+                parameters=[
+                    {"use_sim_time": use_sim_time},
+                    {"condition_topic" : "/gz_ros2_vacuum_gripper_plugin/contact_info"},
+                    # {"robot_name": namespace},
+                    {"base_link": base_link_name},
+                    # {"ee_link": ee_link_name},
+                    # robot_description,
+                ],
+            ),
+
         ],
     )
 

rbs_skill_servers/src/twist_cmd_with_condition.cpp

@@ -0,0 +1,139 @@
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include <chrono>
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/publisher_options.hpp>
+#include <rclcpp/subscription.hpp>
+#include <rclcpp/subscription_options.hpp>
+#include <rclcpp/timer.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+
+namespace rbs_skill_actions {
+
+using TwistCmd = rbs_skill_interfaces::action::TwistControlWithCondition;
+using ServerGoalHandle = rclcpp_action::ServerGoalHandle<TwistCmd>;
+
+class TwistCmdWithCondition : public SkillBase<TwistCmd> {
+public:
+  explicit TwistCmdWithCondition(
+      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase<TwistCmd>("twist_cmd", options),
+        m_last_msg(std::make_shared<std_msgs::msg::Bool>()) {
+
+    this->declare_parameter("condition_topic", "");
+    this->declare_parameter("base_link", "");
+
+    auto cbg =
+        this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+
+    rclcpp::SubscriptionOptions sopt;
+    sopt.callback_group = cbg;
+
+    rclcpp::PublisherOptions popt;
+    popt.callback_group = cbg;
+
+    m_condition_sub = this->create_subscription<std_msgs::msg::Bool>(
+        this->get_parameter("condition_topic").as_string(), 10,
+        std::bind(&TwistCmdWithCondition::conditionCallback, this,
+                  std::placeholders::_1),
+        sopt);
+
+    m_command_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
+        "/cartesian_twist_controller/twist_cmd", 10, popt);
+
+    m_command_timer = this->create_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&TwistCmdWithCondition::publishCmd, this), cbg);
+  }
+
+protected:
+  std::string requiredActionController() override {
+    return "cartesian_twist_controller";
+  }
+
+  std::vector<std::string> requiredParameters() override { return {"joints"}; }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
+
+    m_twist_cmd.header.frame_id = this->get_parameter("base_link").as_string();
+    m_twist_cmd.header.stamp = this->get_clock()->now();
+    m_twist_cmd.twist = m_current_goal->twist_cmd;
+
+    RCLCPP_INFO(
+        this->get_logger(), "Incoming command twist [%f, %f, %f, %f, %f, %f]",
+        m_current_goal->twist_cmd.linear.x, m_current_goal->twist_cmd.linear.y,
+        m_current_goal->twist_cmd.linear.z, m_current_goal->twist_cmd.angular.x,
+        m_current_goal->twist_cmd.angular.y,
+        m_current_goal->twist_cmd.angular.z);
+
+    std::unique_lock<std::mutex> lg(m_condition_mutex);
+
+    if (!m_condition_cv.wait_for(lg, std::chrono::seconds(30),
+                                 [this]() { return m_last_msg->data; })) {
+      if (m_current_goal_handle) {
+        m_current_result->success = false;
+        m_current_goal_handle->abort(m_current_result);
+        RCLCPP_WARN(this->get_logger(),
+                    "Condition was not met in time, action aborted.");
+      }
+      return;
+    }
+    if (m_last_msg->data && m_current_goal_handle) {
+      m_current_result->success = true;
+      m_current_goal_handle->succeed(m_current_result);
+      RCLCPP_INFO(this->get_logger(), "Condition is true, stopping action.");
+    }
+    lg.unlock();
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr m_condition_sub;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr m_command_pub;
+  rclcpp::TimerBase::SharedPtr m_command_timer;
+  geometry_msgs::msg::TwistStamped m_twist_cmd;
+  std_msgs::msg::Bool::SharedPtr m_last_msg;
+  std::mutex m_condition_mutex;
+  std::condition_variable m_condition_cv;
+  bool cond_rising_edge = false;
+  bool cond_falling_edge = false;
+
+  void publishCmd() {
+    if (cond_falling_edge) {
+      m_command_pub->publish(m_twist_cmd);
+    } else {
+      m_twist_cmd.twist.linear.x = 0.0;
+      m_twist_cmd.twist.linear.y = 0.0;
+      m_twist_cmd.twist.linear.z = 0.0;
+      m_twist_cmd.twist.angular.x = 0.0;
+      m_twist_cmd.twist.angular.y = 0.0;
+      m_twist_cmd.twist.angular.z = 0.0;
+      m_command_pub->publish(m_twist_cmd);
+    }
+  }
+
+  void conditionCallback(const std_msgs::msg::Bool &msg) {
+    std::lock_guard<std::mutex> lg(m_condition_mutex);
+
+    cond_rising_edge = (!m_last_msg->data && msg.data);
+    cond_falling_edge = (m_last_msg->data && !msg.data);
+    if (!m_last_msg->data && !msg.data) {
+        cond_falling_edge = true; 
+    }
+    if (cond_falling_edge or cond_rising_edge) {
+      *m_last_msg = msg;
+      m_condition_cv.notify_one();
+    }
+  }
+};
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::TwistCmdWithCondition);