diff --git a/rbs_skill_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt index c2f272f..cb29df5 100644 --- a/rbs_skill_interfaces/CMakeLists.txt +++ b/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" diff --git a/rbs_skill_interfaces/action/TwistControlWithCondition.action b/rbs_skill_interfaces/action/TwistControlWithCondition.action new file mode 100644 index 0000000..71d4026 --- /dev/null +++ b/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 diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 71afef3..3bf91bb 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/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}) diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index 02ad676..e2ee73a 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/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, + ], + ), + ], ) diff --git a/rbs_skill_servers/src/twist_cmd_with_condition.cpp b/rbs_skill_servers/src/twist_cmd_with_condition.cpp new file mode 100644 index 0000000..705de44 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rbs_skill_actions { + +using TwistCmd = rbs_skill_interfaces::action::TwistControlWithCondition; +using ServerGoalHandle = rclcpp_action::ServerGoalHandle; + +class TwistCmdWithCondition : public SkillBase { +public: + explicit TwistCmdWithCondition( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) + : SkillBase("twist_cmd", options), + m_last_msg(std::make_shared()) { + + 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( + this->get_parameter("condition_topic").as_string(), 10, + std::bind(&TwistCmdWithCondition::conditionCallback, this, + std::placeholders::_1), + sopt); + + m_command_pub = this->create_publisher( + "/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 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 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::SharedPtr m_condition_sub; + rclcpp::Publisher::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 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);