diff --git a/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.cpp b/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.cpp index 7b4913cd35..dfa5553f48 100644 --- a/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.cpp @@ -69,10 +69,18 @@ void MoveGroupManipulationAgentService::initialize() auto node = context_->moveit_cpp_->getNode(); callback_group_action_client_ = node->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive); + + // Gazebo + gz_attach_pub_ = node->create_publisher( + "/gz/attach_box1", rclcpp::QoS(1)); + + gz_detach_pub_ = node->create_publisher( + "/gz/detach_box1", rclcpp::QoS(1)); + attach_object_action_client_ = rclcpp_action::create_client( node, "/attach_object", callback_group_action_client_); - get_arm_position_service_ = context_->moveit_cpp_->getNode()->create_service( + get_arm_position_service_ = context_->moveit_cpp_->getNode()->create_service( "get_arm_position", [this](const std::shared_ptr& request_header, const std::shared_ptr& req, const std::shared_ptr& res) { @@ -348,17 +356,24 @@ void MoveGroupManipulationAgentService::initialize() RCLCPP_ERROR(LOGGER, "Object Attachment goal execution time out..."); return; } + + auto pub_empty = [](auto & pub) { + std_msgs::msg::Empty msg; + pub->publish(msg); + }; // The final result auto result = future_result.get(); if (result.result->outcome.find("attached") != std::string::npos) { + pub_empty(gz_attach_pub_); RCLCPP_INFO(LOGGER, "Object '%s' attached successfully to arm '%s'", req->object_id.name.c_str(), req->new_arm.c_str()); res->error_info.description = result.result->outcome; } else if (result.result->outcome.find("Detached") != std::string::npos) { + pub_empty(gz_detach_pub_); RCLCPP_INFO(LOGGER, "Object '%s' detached successfully from arm '%s'", req->object_id.name.c_str(), req->prev_arm.c_str()); res->error_info.description = result.result->outcome; } diff --git a/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.h b/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.h index 628f03dbf2..388a0b52d8 100644 --- a/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/manipulation_agent_capability.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -64,7 +65,8 @@ MoveGroupManipulationAgentService(); std::string StrLowerCase (const std::string & value); bool HasSubStrI (const std::string & value, const std::string & query); int ArmIdx (const std::string & name); - + rclcpp::Publisher::SharedPtr gz_attach_pub_; + rclcpp::Publisher::SharedPtr gz_detach_pub_; rclcpp::Service::SharedPtr get_arm_position_service_; rclcpp::Service::SharedPtr get_arm_pose_service_; rclcpp::Service::SharedPtr get_gripper_position_service_;