clang format

moveit_task_constructor_dexnet/CMakeLists.txt

@@ -27,7 +27,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(

moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h

@@ -42,7 +42,7 @@
 #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
@@ -107,10 +107,9 @@ class GraspDetection
   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/src/grasp_detection.cpp

@@ -78,8 +78,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 +100,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 +158,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 +191,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_);
   }
 }