New grasp pose generator stage

core/CMakeLists.txt

@@ -4,7 +4,9 @@ project(moveit_task_constructor_core)
 find_package(Boost REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
 	tf2_eigen
+	actionlib
 	geometry_msgs
+	grasping_msgs
 	moveit_core
 	moveit_ros_planning
 	moveit_ros_planning_interface
@@ -22,7 +24,9 @@ catkin_package(
 	INCLUDE_DIRS
 		include
 	CATKIN_DEPENDS
+		actionlib
 		geometry_msgs
+		grasping_msgs
 		moveit_core
 		moveit_task_constructor_msgs
 		rviz_marker_tools

core/include/moveit/task_constructor/stages/action_base.h

@@ -0,0 +1,100 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2020 PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Boston Cleek
+   Desc:   Abstact class for stages using a simple action client.
+*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <limits>
+
+#include <actionlib/client/simple_action_client.h>
+#include <grasping_msgs/GraspPlanningAction.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+/** @brief Interface allowing stages to use a simple action client */
+class ActionBase
+{
+public:
+	/**
+	 * @brief Constructor
+	 * @param action_name - action namespace
+	 * @param spin_thread - spins a thread to service this action's subscriptions
+	 * @param goal_timeout - goal to completed time out (0 is considered infinite timeout)
+	 * @param server_timeout - connection to server time out (0 is considered infinite timeout)
+	 * @details Initialize the action client and time out parameters
+	 */
+	ActionBase(const std::string& action_name, bool spin_thread, double goal_timeout, double server_timeout);
+
+	/**
+	 * @brief Constructor
+	 * @param action_name - action namespace
+	 * @param spin_thread - spins a thread to service this action's subscriptions
+	 * @details Initialize the action client and time out parameters to infinity
+	 */
+	ActionBase(const std::string& action_name, bool spin_thread);
+
+	/* @brief Destructor */
+	virtual ~ActionBase() = default;
+
+	/* @brief Called when goal becomes active */
+	virtual void activeCallback() = 0;
+
+	/**
+	 * @brief Called every time feedback is received for the goal
+	 * @param feedback - pointer to the feedback message
+	 */
+	virtual void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) = 0;
+
+	/**
+	 * @brief Called once when the goal completes
+	 * @param state - state info for goal
+	 * @param result - pointer to result message
+	 */
+	virtual void doneCallback(const actionlib::SimpleClientGoalState& state,
+	                          const grasping_msgs::GraspPlanningResultConstPtr& result) = 0;
+
+protected:
+	ros::NodeHandle nh_;
+	std::string action_name_;  // action name space
+	std::unique_ptr<actionlib::SimpleActionClient<grasping_msgs::GraspPlanningAction>> clientPtr_;  // action client
+	double server_timeout_, goal_timeout_;  // connection and goal completed time out
+};
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/include/moveit/task_constructor/stages/grasp_provider.h

@@ -0,0 +1,107 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2020 PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Boston Cleek
+   Desc:   Grasp generator stage
+*/
+
+#pragma once
+
+#include <functional>
+#include <mutex>
+
+#include <moveit/task_constructor/stages/generate_pose.h>
+#include <moveit/task_constructor/stages/action_base.h>
+#include <moveit/task_constructor/storage.h>
+#include <moveit/task_constructor/marker_tools.h>
+#include <rviz_marker_tools/marker_creation.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+/** @brief Generate grasp candidates using deep learning approaches */
+class GraspProvider : public GeneratePose, ActionBase
+{
+public:
+	/**
+	 * @brief Constructor
+	 * @param action_name - action namespace
+	 * @param stage_name - name of stage
+	 * @param goal_timeout - goal to completed time out (0 is considered infinite timeout)
+	 * @param server_timeout - connection to server time out (0 is considered infinite timeout)
+	 * @details Initialize the client and connect to server
+	 */
+	GraspProvider(const std::string& action_name, const std::string& stage_name = "grasp provider",
+	              double goal_timeout = 0.0, double server_timeout = 0.0);
+
+	/**
+	 * @brief Composes the action goal and sends to server
+	 */
+	void composeGoal();
+
+	/**
+	 * @brief Monitors status of action goal
+	 * @return true if grasp candidates are received within (optional) timeout
+	 * @details This is a blocking call. It will wait until either grasp candidates
+	 *          are received or the timeout has been reached.
+	 */
+	bool monitorGoal();
+
+	void activeCallback() override;
+	void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) override;
+	void doneCallback(const actionlib::SimpleClientGoalState& state,
+	                  const grasping_msgs::GraspPlanningResultConstPtr& result) override;
+
+	void init(const core::RobotModelConstPtr& robot_model) override;
+	void compute() override;
+
+	void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
+	void setObject(const std::string& object) { setProperty("object", object); }
+
+	void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
+	void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
+	void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
+	void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
+
+protected:
+	void onNewSolution(const SolutionBase& s) override;
+
+private:
+	std::mutex grasp_mutex_;  // Protects grasp candidates
+	std::atomic_bool found_candidates_;  // Flag indicates the discovery of grasps
+	std::vector<moveit_msgs::Grasp> grasp_candidates_;  // Grasp Candidates
+};
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/package.xml

@@ -13,6 +13,8 @@
 	<exec_depend>roscpp</exec_depend>
 
 	<depend>tf2_eigen</depend>
+	<depend>actionlib</depend>
+	<depend>grasping_msgs</depend>
 	<depend>geometry_msgs</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_planning</depend>

core/src/stages/CMakeLists.txt

@@ -2,12 +2,14 @@ add_library(${PROJECT_NAME}_stages
 	${PROJECT_INCLUDE}/stages/modify_planning_scene.h
 	${PROJECT_INCLUDE}/stages/fix_collision_objects.h
 
+	${PROJECT_INCLUDE}/stages/action_base.h
 	${PROJECT_INCLUDE}/stages/current_state.h
 	${PROJECT_INCLUDE}/stages/fixed_state.h
 	${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
 	${PROJECT_INCLUDE}/stages/generate_pose.h
 	${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
 	${PROJECT_INCLUDE}/stages/generate_place_pose.h
+	${PROJECT_INCLUDE}/stages/grasp_provider.h
 	${PROJECT_INCLUDE}/stages/compute_ik.h
 	${PROJECT_INCLUDE}/stages/passthrough.h
 	${PROJECT_INCLUDE}/stages/predicate_filter.h
@@ -22,12 +24,14 @@ add_library(${PROJECT_NAME}_stages
 	modify_planning_scene.cpp
 	fix_collision_objects.cpp
 
+	action_base.cpp
 	current_state.cpp
 	fixed_state.cpp
 	fixed_cartesian_poses.cpp
 	generate_pose.cpp
 	generate_grasp_pose.cpp
 	generate_place_pose.cpp
+	grasp_provider.cpp
 	compute_ik.cpp
 	passthrough.cpp
 	predicate_filter.cpp
@@ -39,6 +43,7 @@ add_library(${PROJECT_NAME}_stages
 	simple_grasp.cpp
 	pick.cpp
 )
+
 target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
 
 add_library(${PROJECT_NAME}_stage_plugins

core/src/stages/action_base.cpp

@@ -0,0 +1,63 @@
+/*********************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2021 PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ *  * Neither the name of the copyright holder nor the names of its
+ *    contributors may be used to endorse or promote products derived from
+ *    this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Boston Cleek
+   Desc:   Abstact class for stages using a simple action client.
+*/
+
+#include <moveit/task_constructor/stages/action_base.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+constexpr char LOGNAME[] = "action_base";
+
+ActionBase::ActionBase(const std::string& action_name, bool spin_thread, double goal_timeout, double server_timeout)
+  : action_name_(action_name), server_timeout_(server_timeout), goal_timeout_(goal_timeout) {
+	clientPtr_ = std::make_unique<actionlib::SimpleActionClient<grasping_msgs::GraspPlanningAction>>(nh_, action_name_,
+	                                                                                                 spin_thread);
+
+	// Negative timeouts are set to zero
+	server_timeout_ = server_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : server_timeout_;
+	goal_timeout_ = goal_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : goal_timeout_;
+
+	ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for connection to grasp generation action server...");
+	clientPtr_->waitForServer(ros::Duration(server_timeout_));
+	ROS_DEBUG_STREAM_NAMED(LOGNAME, "Connected to server");
+}
+
+ActionBase::ActionBase(const std::string& action_name, bool spin_thread)
+  : ActionBase::ActionBase(action_name, spin_thread, 0.0, 0.0) {}
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit