Update Demo to use GraspProvider interface and grasping_msgs::GraspPlanningAction

deep_grasp_task/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS
 	moveit_task_constructor_msgs
   roscpp
 	rosparam_shortcuts
+  rviz_marker_tools
 )
 
 ###################################
@@ -27,6 +28,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
  INCLUDE_DIRS include
  CATKIN_DEPENDS
+  actionlib
 	moveit_task_constructor_msgs
  	roscpp
 )

deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h

@@ -56,15 +56,14 @@
 #include <moveit/task_constructor/stages/generate_grasp_pose.h>
 #include <moveit/task_constructor/stages/generate_pose.h>
 #include <moveit/task_constructor/stages/generate_place_pose.h>
-#include <moveit/task_constructor/stages/deep_grasp_pose.h>
+#include <moveit/task_constructor/stages/grasp_provider.h>
 #include <moveit/task_constructor/stages/modify_planning_scene.h>
 #include <moveit/task_constructor/stages/move_relative.h>
 #include <moveit/task_constructor/stages/move_to.h>
 #include <moveit/task_constructor/stages/predicate_filter.h>
 #include <moveit/task_constructor/solvers/cartesian_path.h>
 #include <moveit/task_constructor/solvers/pipeline_planner.h>
 #include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
-#include <moveit_task_constructor_msgs/SampleGraspPosesAction.h>
 
 namespace deep_grasp_task
 {

deep_grasp_task/package.xml

@@ -17,4 +17,16 @@
   <depend>moveit_core</depend>
   <depend>roscpp</depend>
   <depend>rosparam_shortcuts</depend>
+  <depend>rviz_marker_tools</depend>
+
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>moveit_task_constructor_demo</exec_depend>
+  <exec_depend>panda_moveit_config</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
 </package>

deep_grasp_task/src/deep_grasp_demo.cpp

@@ -49,7 +49,7 @@
 #include <iostream>
 
 #include <geometric_shapes/shape_operations.h>
-#include <moveit_task_constructor_msgs/SampleGraspPosesAction.h>
+#include <grasping_msgs/GraspPlanningAction.h>
 #include <actionlib/client/simple_action_client.h>
 
 constexpr char LOGNAME[] = "deep_grasp_demo";
@@ -167,16 +167,6 @@ moveit_msgs::CollisionObject createObjectMesh()
   object.mesh_poses.emplace_back(pose);
   object.operation = moveit_msgs::CollisionObject::ADD;
 
-  // moveit_msgs::CollisionObject object;
-  // object.id = object_name;
-  // object.header.frame_id = object_reference_frame;
-  // object.primitives.resize(1);
-  // object.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
-  // object.primitives[0].dimensions = object_dimensions;
-  // pose.position.z += 0.5 * object_dimensions[0];
-  // object.primitive_poses.push_back(pose);
-  // object.operation = moveit_msgs::CollisionObject::ADD;
-
   return object;
 }
 

deep_grasp_task/src/deep_pick_place_task.cpp

@@ -204,8 +204,7 @@ void DeepPickPlaceTask::init()
   ---- *               Generate Grasp Pose                *
      ***************************************************/
     {
-      auto stage = std::make_unique<stages::DeepGraspPose<moveit_task_constructor_msgs::SampleGraspPosesAction>>(
-          action_name_, "generate grasp pose");
+      auto stage = std::make_unique<stages::GraspProvider>(action_name_, "generate grasp pose");
       stage->properties().configureInitFrom(Stage::PARENT);
       stage->properties().set("marker_ns", "grasp_pose");
       stage->setPreGraspPose(hand_open_pose_);

moveit_task_constructor_dexnet/CMakeLists.txt

@@ -15,9 +15,10 @@ find_package(catkin REQUIRED COMPONENTS
 	actionlib
 	cv_bridge
 	geometry_msgs
+  grasping_msgs
 	image_transport
 	message_generation
-	moveit_task_constructor_msgs
+  moveit_msgs
 	roscpp
 	rosparam_shortcuts
 	rospy
@@ -27,7 +28,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 # System dependencies are found with CMake's conventions
 find_package(Eigen3 REQUIRED)
-find_package(OpenCV 3.2 REQUIRED)
+find_package(OpenCV REQUIRED)
 
 # Service files
 add_service_files(
@@ -49,9 +50,11 @@ generate_messages(
 catkin_package(
  INCLUDE_DIRS include
  CATKIN_DEPENDS
+  actionlib
  	geometry_msgs
+  grasping_msgs
 	message_runtime
-	moveit_task_constructor_msgs
+  moveit_msgs
  	roscpp
 	rospy
 	sensor_msgs

moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h

@@ -42,75 +42,74 @@
 #include <Eigen/Geometry>
 
 // Action Server
-#include <moveit_task_constructor_msgs/SampleGraspPosesAction.h>
+#include <grasping_msgs/GraspPlanningAction.h>
 #include <actionlib/server/simple_action_server.h>
 
 namespace moveit_task_constructor_dexnet
 {
 constexpr char LOGNAME[] = "grasp_image_detection";
 
 /**
-* @brief Generates grasp poses for a generator stage with MTC
-* @details Interfaces with the GQ-CNN and Dex-Net data sets using ROS messages
-*          and interfaces with MTC using an Action Server.
-*/
+ * @brief Generates grasp poses for a generator stage with MTC
+ * @details Interfaces with the GQ-CNN and Dex-Net data sets using ROS messages
+ *          and interfaces with MTC using an Action Server.
+ */
 class GraspDetection
 {
 public:
   /**
-  * @brief Constructor
-  * @param nh - node handle
-  * @details loads parameter and registers callbacks for the action server
-  */
+   * @brief Constructor
+   * @param nh - node handle
+   * @details loads parameter and registers callbacks for the action server
+   */
   GraspDetection(const ros::NodeHandle& nh);
 
 private:
   /**
-  * @brief Loads parameters for action server, image data, and relevant transformations
-  */
+   * @brief Loads parameters for action server, image data, and relevant transformations
+   */
   void loadParameters();
 
   /**
-  * @brief Initialize action server callbacks and Image service client (optional)
-  */
+   * @brief Initialize action server callbacks and Image service client (optional)
+   */
   void init();
 
   /**
-  * @brief Action server goal callback
-  * @details Accepts goal from client and samples grasp candidates
-  */
+   * @brief Action server goal callback
+   * @details Accepts goal from client and samples grasp candidates
+   */
   void goalCallback();
 
   /**
-  * @brief Preempt callback
-  * @details Preempts goal
-  */
+   * @brief Preempt callback
+   * @details Preempts goal
+   */
   void preemptCallback();
 
   /**
-  * @brief Requests images by calling the save_images service
-  * @details If Images are not loaded from the data directory they need to be
-  *          requested. This assumes a camera is publishing the images.
-  */
+   * @brief Requests images by calling the save_images service
+   * @details If Images are not loaded from the data directory they need to be
+   *          requested. This assumes a camera is publishing the images.
+   */
   void requestImages();
 
   /**
-  * @brief Samples grasp candidates using a GQ-CNN and Dex-Net model weights/data sets
-  * @details Compose grasp candidates, the candidates are sent back to the client
-  *          using the feedback message. The calls the gqcnn_grasp service to
-  *          execute the learned policy.
-  */
+   * @brief Samples grasp candidates using a GQ-CNN and Dex-Net model weights/data sets
+   * @details Compose grasp candidates, the candidates are sent back to the client
+   *          using the feedback message. The calls the gqcnn_grasp service to
+   *          execute the learned policy.
+   */
   void sampleGrasps();
 
 private:
   ros::NodeHandle nh_;               // node handle
   ros::ServiceClient gqcnn_client_;  // gqcnn service client
   ros::ServiceClient image_client_;  // image saving service client
 
-  std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::SampleGraspPosesAction>>
-      server_;                                                       // action server
-  moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_;  // action feedback message
-  moveit_task_constructor_msgs::SampleGraspPosesResult result_;      // action result message
+  std::unique_ptr<actionlib::SimpleActionServer<grasping_msgs::GraspPlanningAction>> server_;  // action server
+  grasping_msgs::GraspPlanningFeedback feedback_;  // action feedback message
+  grasping_msgs::GraspPlanningResult result_;      // action result message
 
   std::string goal_name_;    // action name
   std::string action_name_;  // action namespace

moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/image_server.h

@@ -49,54 +49,54 @@ namespace moveit_task_constructor_dexnet
 constexpr char LOGNAME[] = "image_server";
 
 /**
-* @brief Provides a service for saving RGB and depth images
-*/
+ * @brief Provides a service for saving RGB and depth images
+ */
 class ImageServer
 {
 public:
   /**
-  * @brief Constructor
-  * @param nh - node handle
-  */
+   * @brief Constructor
+   * @param nh - node handle
+   */
   ImageServer(ros::NodeHandle& nh);
 
 private:
   /**
-  * @brief Loads parameters
-  */
+   * @brief Loads parameters
+   */
   void loadParameters();
 
   /**
-  * @brief Initialize ROS communication
-  */
+   * @brief Initialize ROS communication
+   */
   void init();
 
   /**
-  * @brief RGB image callback
-  * @param msg - GGB image
-  */
+   * @brief RGB image callback
+   * @param msg - GGB image
+   */
   void colorCallback(const sensor_msgs::Image::ConstPtr& msg);
 
   /**
-  * @brief Depth image callback
-  * @param msg - Depth image
-  */
+   * @brief Depth image callback
+   * @param msg - Depth image
+   */
   void depthCallback(const sensor_msgs::Image::ConstPtr& msg);
 
   /**
-  * @brief Service callback for saving images
-  * @param req - Service request contains the file name
-  * @param res [out] - Service result true if image type is saved
-  */
+   * @brief Service callback for saving images
+   * @param req - Service request contains the file name
+   * @param res [out] - Service result true if image type is saved
+   */
   bool saveCallback(moveit_task_constructor_dexnet::Images::Request& req,
                     moveit_task_constructor_dexnet::Images::Response& res);
 
   /**
-  * @brief Saves image based on encoding and to specified file
-  * @param msg - Image
-  * @param image_name - File name of image
-  * @details Images are saved as CV_8UC3 (BGR8) by OpenCV by default
-  */
+   * @brief Saves image based on encoding and to specified file
+   * @param msg - Image
+   * @param image_name - File name of image
+   * @details Images are saved as CV_8UC3 (BGR8) by OpenCV by default
+   */
   bool saveImage(const sensor_msgs::Image::ConstPtr& msg, const std::string& image_name);
 
 private:

moveit_task_constructor_dexnet/package.xml

@@ -15,12 +15,15 @@
   <depend>actionlib</depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
-  <depend>moveit_task_constructor_msgs</depend>
+  <depend>grasping_msgs</depend>
   <depend>image_transport</depend>
   <depend>roscpp</depend>
   <depend>rospy</depend>
   <depend>rosparam_shortcuts</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>moveit_msgs</depend>
+
+  <exec_depend>deep_grasp_task</exec_depend>
   <exec_depend>message_runtime</exec_depend>
 </package>

moveit_task_constructor_dexnet/src/grasp_detection.cpp

@@ -38,6 +38,7 @@
 #include <ros/ros.h>
 #include <rosparam_shortcuts/rosparam_shortcuts.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <moveit_msgs/Grasp.h>
 
 // C++
 #include <vector>
@@ -78,8 +79,7 @@ void GraspDetection::loadParameters()
 void GraspDetection::init()
 {
   // action server
-  server_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::SampleGraspPosesAction>(
-      nh_, action_name_, false));
+  server_.reset(new actionlib::SimpleActionServer<grasping_msgs::GraspPlanningAction>(nh_, action_name_, false));
   server_->registerGoalCallback(std::bind(&GraspDetection::goalCallback, this));
   server_->registerPreemptCallback(std::bind(&GraspDetection::preemptCallback, this));
   server_->start();
@@ -101,7 +101,7 @@ void GraspDetection::init()
 
 void GraspDetection::goalCallback()
 {
-  goal_name_ = server_->acceptNewGoal()->action_name;
+  goal_name_ = server_->acceptNewGoal()->object.name;
   ROS_INFO_NAMED(LOGNAME, "New goal accepted: %s", goal_name_.c_str());
 
   // save images
@@ -159,7 +159,6 @@ void GraspDetection::sampleGrasps()
     if (grasp_candidates.empty())
     {
       ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found");
-      result_.grasp_state = "failed";
       server_->setAborted(result_);
       return;
     }
@@ -193,24 +192,22 @@ void GraspDetection::sampleGrasps()
       grasp_pose.pose.orientation.z = rot.z();
 
       // send feedback to action client
-      feedback_.grasp_candidates.emplace_back(grasp_pose);
+      moveit_msgs::Grasp current_grasp;
+      current_grasp.grasp_pose = grasp_pose;
 
       // Q_value (probability of success)
       // cost = 1.0 - Q_value, to represent cost
-      const double cost = 1.0 - q_values.at(i);
-      // ROS_INFO_NAMED(LOGNAME, "ID: %u Cost: %f", i, cost);
-      feedback_.costs.emplace_back(cost);
+      current_grasp.grasp_quality = 1.0 - q_values.at(i);
+
+      feedback_.grasps.emplace_back(current_grasp);
     }
 
     server_->publishFeedback(feedback_);
-    result_.grasp_state = "success";
     server_->setSucceeded(result_);
   }
-
   else
   {
     ROS_ERROR_NAMED(LOGNAME, "Failed to call gqcnn_grasp service");
-    result_.grasp_state = "failed";
     server_->setAborted(result_);
   }
 }