diff --git a/deep_grasp_task/CMakeLists.txt b/deep_grasp_task/CMakeLists.txt index 853e852..952b2c7 100644 --- a/deep_grasp_task/CMakeLists.txt +++ b/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 ) diff --git a/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h b/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h index f2fc514..ccb09ac 100644 --- a/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h +++ b/deep_grasp_task/include/deep_grasp_task/deep_pick_place_task.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -64,7 +64,6 @@ #include #include #include -#include namespace deep_grasp_task { diff --git a/deep_grasp_task/package.xml b/deep_grasp_task/package.xml index c4b4ef1..ab8d9c7 100644 --- a/deep_grasp_task/package.xml +++ b/deep_grasp_task/package.xml @@ -17,4 +17,16 @@ moveit_core roscpp rosparam_shortcuts + rviz_marker_tools + + gazebo_ros + joint_state_publisher + moveit_task_constructor_demo + panda_moveit_config + robot_state_publisher + rviz + tf2_ros + topic_tools + xacro + diff --git a/deep_grasp_task/src/deep_grasp_demo.cpp b/deep_grasp_task/src/deep_grasp_demo.cpp index ec4a3be..545b4de 100644 --- a/deep_grasp_task/src/deep_grasp_demo.cpp +++ b/deep_grasp_task/src/deep_grasp_demo.cpp @@ -49,7 +49,7 @@ #include #include -#include +#include #include 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; } diff --git a/deep_grasp_task/src/deep_pick_place_task.cpp b/deep_grasp_task/src/deep_pick_place_task.cpp index b017948..023734f 100644 --- a/deep_grasp_task/src/deep_pick_place_task.cpp +++ b/deep_grasp_task/src/deep_pick_place_task.cpp @@ -204,8 +204,7 @@ void DeepPickPlaceTask::init() ---- * Generate Grasp Pose * ***************************************************/ { - auto stage = std::make_unique>( - action_name_, "generate grasp pose"); + auto stage = std::make_unique(action_name_, "generate grasp pose"); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose(hand_open_pose_); diff --git a/moveit_task_constructor_dexnet/CMakeLists.txt b/moveit_task_constructor_dexnet/CMakeLists.txt index 22bb879..cddc7d3 100644 --- a/moveit_task_constructor_dexnet/CMakeLists.txt +++ b/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 diff --git a/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h b/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h index 5183941..a1ecb49 100644 --- a/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h +++ b/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/grasp_detection.h @@ -42,7 +42,7 @@ #include // Action Server -#include +#include #include namespace moveit_task_constructor_dexnet @@ -50,56 +50,56 @@ 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: @@ -107,10 +107,9 @@ class GraspDetection ros::ServiceClient gqcnn_client_; // gqcnn service client ros::ServiceClient image_client_; // image saving service client - std::unique_ptr> - server_; // action server - moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_; // action feedback message - moveit_task_constructor_msgs::SampleGraspPosesResult result_; // action result message + std::unique_ptr> 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 diff --git a/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/image_server.h b/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/image_server.h index 35ae80c..b4facd8 100644 --- a/moveit_task_constructor_dexnet/include/moveit_task_constructor_dexnet/image_server.h +++ b/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: diff --git a/moveit_task_constructor_dexnet/package.xml b/moveit_task_constructor_dexnet/package.xml index 80458f4..1fc65ed 100644 --- a/moveit_task_constructor_dexnet/package.xml +++ b/moveit_task_constructor_dexnet/package.xml @@ -15,12 +15,15 @@ actionlib cv_bridge geometry_msgs - moveit_task_constructor_msgs + grasping_msgs image_transport roscpp rospy rosparam_shortcuts sensor_msgs std_msgs + moveit_msgs + + deep_grasp_task message_runtime diff --git a/moveit_task_constructor_dexnet/src/grasp_detection.cpp b/moveit_task_constructor_dexnet/src/grasp_detection.cpp index ed8553a..204b87e 100644 --- a/moveit_task_constructor_dexnet/src/grasp_detection.cpp +++ b/moveit_task_constructor_dexnet/src/grasp_detection.cpp @@ -38,6 +38,7 @@ #include #include #include +#include // C++ #include @@ -78,8 +79,7 @@ void GraspDetection::loadParameters() void GraspDetection::init() { // action server - server_.reset(new actionlib::SimpleActionServer( - nh_, action_name_, false)); + server_.reset(new actionlib::SimpleActionServer(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_); } } diff --git a/moveit_task_constructor_gpd/CMakeLists.txt b/moveit_task_constructor_gpd/CMakeLists.txt index 39c3895..8fabd9b 100644 --- a/moveit_task_constructor_gpd/CMakeLists.txt +++ b/moveit_task_constructor_gpd/CMakeLists.txt @@ -15,8 +15,9 @@ find_package(catkin REQUIRED COMPONENTS actionlib eigen_conversions geometry_msgs + grasping_msgs message_generation - moveit_task_constructor_msgs + moveit_msgs roscpp rosparam_shortcuts sensor_msgs @@ -26,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS # System dependencies are found with CMake's conventions find_package(Eigen3 REQUIRED) find_package(PCL 1.8 REQUIRED) +find_package(OpenCV REQUIRED) find_library(GPD_LIB NAMES gpd PATHS /usr/local/lib PATH_SUFFIXES lib NO_DEFAULT_PATH) if (GPD_LIB) @@ -60,14 +62,17 @@ catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS + actionlib eigen_conversions geometry_msgs + grasping_msgs message_runtime - moveit_task_constructor_msgs + moveit_msgs sensor_msgs std_msgs DEPENDS EIGEN3 + OpenCV PCL ) @@ -82,6 +87,7 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${GPD_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) include_directories( @@ -107,6 +113,7 @@ add_dependencies(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} ) # Declare a C++ executable @@ -141,12 +148,14 @@ target_link_libraries(grasp_cloud_detection ${Eigen3_LIBRARIES} ${PCL_LIBRARIES} ${GPD_LIB} + ${OpenCV_LIBRARIES} ) target_link_libraries(point_cloud_server ${catkin_LIBRARIES} ${PROJECT_NAME} ${PCL_LIBRARIES} + ${OpenCV_LIBRARIES} ) ############# diff --git a/moveit_task_constructor_gpd/config/gpd_config.yaml b/moveit_task_constructor_gpd/config/gpd_config.yaml index a12f17b..883e876 100644 --- a/moveit_task_constructor_gpd/config/gpd_config.yaml +++ b/moveit_task_constructor_gpd/config/gpd_config.yaml @@ -30,7 +30,7 @@ image_size = 60 image_num_channels = 15 # Path to directory that contains neural network parameters -weights_file = /home/bostoncleek/GPD/gpd/models/lenet/15channels/params/ +weights_file = /home/boston/libraries/gpd/models/lenet/15channels/params/ # Preprocessing of point cloud # voxelize: if the cloud gets voxelized/downsampled diff --git a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_server.h b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_server.h index 5d1cc9b..85c1034 100644 --- a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_server.h +++ b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_server.h @@ -50,47 +50,47 @@ namespace moveit_task_constructor_gpd constexpr char LOGNAME[] = "cloud_server"; /** -* @brief Provides a service for saving and process a point cloud -* @details When the service is called the point cloud received through -* the point cloud topic is saved. Optionally, either the ground plane -* is removed and/or points outside the specified cartesian limits -* are removed. The resulting cloud is published. -*/ + * @brief Provides a service for saving and process a point cloud + * @details When the service is called the point cloud received through + * the point cloud topic is saved. Optionally, either the ground plane + * is removed and/or points outside the specified cartesian limits + * are removed. The resulting cloud is published. + */ class CloudServer { public: /** - * @brief Constructor - * @param nh - node handle - */ + * @brief Constructor + * @param nh - node handle + */ CloudServer(ros::NodeHandle& nh); private: /** - * @brief Loads parameters - */ + * @brief Loads parameters + */ void loadParameters(); /** - * @brief Initialize ROS communication - */ + * @brief Initialize ROS communication + */ void init(); /** - * @brief Point cloud call back - * @param msg - point cloud message - * @details Optionally, either the ground plane - * is removed and/or points outside the specified cartesian limits - * are removed. The resulting cloud is published. - */ + * @brief Point cloud call back + * @param msg - point cloud message + * @details Optionally, either the ground plane + * is removed and/or points outside the specified cartesian limits + * are removed. The resulting cloud is published. + */ void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); /** - * @brief Service callback for saving a point cloud - * @param req - Service request contains the file name - * @return true when called - * @details The response is empty - */ + * @brief Service callback for saving a point cloud + * @param req - Service request contains the file name + * @return true when called + * @details The response is empty + */ bool saveCallback(moveit_task_constructor_gpd::PointCloud::Request& req, moveit_task_constructor_gpd::PointCloud::Response&); diff --git a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_utils.h b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_utils.h index a809dce..296143b 100644 --- a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_utils.h +++ b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/cloud_utils.h @@ -49,17 +49,17 @@ typedef pcl::PointCloud PointCloudRGBA; typedef pcl::PointCloud PointCloudRGB; /** -* @brief Segments objects from table plane -* @param [out] cloud - Segemented point cloud XYZRGB -*/ + * @brief Segments objects from table plane + * @param [out] cloud - Segemented point cloud XYZRGB + */ void removeTable(PointCloudRGB::Ptr cloud); /** -* @brief Removes points outside the specified cartesian limits -* @param xyz_lower - 3dim vector x,y,z lower limits on cloud -* @param xyz_upper - 3dim vector x,y,z upper limits on cloud -* @param [out] cloud - cloud XYZRGB -*/ + * @brief Removes points outside the specified cartesian limits + * @param xyz_lower - 3dim vector x,y,z lower limits on cloud + * @param xyz_upper - 3dim vector x,y,z upper limits on cloud + * @param [out] cloud - cloud XYZRGB + */ void passThroughFilter(const std::vector& xyz_lower, const std::vector& xyz_upper, PointCloudRGB::Ptr cloud); } // namespace moveit_task_constructor_gpd diff --git a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h index ec98b42..8bcd9b5 100644 --- a/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h +++ b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h @@ -51,7 +51,7 @@ #include // Action Server -#include +#include #include namespace moveit_task_constructor_gpd @@ -59,10 +59,10 @@ namespace moveit_task_constructor_gpd constexpr char LOGNAME[] = "grasp_pose_detection"; /** -* @brief Generates grasp poses for a generator stage with MTC -* @details Interfaces with the GPD lib 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 GPD lib using ROS messages and interfaces + * with MTC using an Action Server + */ class GraspDetection { public: @@ -76,43 +76,43 @@ class GraspDetection private: /** - * @brief Loads parameters for action server, GPD, and relevant transformations - */ + * @brief Loads parameters for action server, GPD, and relevant transformations + */ void loadParameters(); /** - * @brief Initialize action server callbacks and GPD - * @details The point cloud (frame: panda_link0) is loaded from a file and - * the camera's origin relative to the point cloud is assumed to be at (0,0,0). - */ + * @brief Initialize action server callbacks and GPD + * @details The point cloud (frame: panda_link0) is loaded from a file and + * the camera's origin relative to the point cloud is assumed to be at (0,0,0). + */ 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 Samples grasp candidates using GPD - * @details Compose grasp candidates, the candidates are sent back to the client - * using the feedback message. Only candidates with a positive grasp - * score are used. If there is at least one candidate with a positive - * score the result is set to success else it is a failure. - */ + * @brief Samples grasp candidates using GPD + * @details Compose grasp candidates, the candidates are sent back to the client + * using the feedback message. Only candidates with a positive grasp + * score are used. If there is at least one candidate with a positive + * score the result is set to success else it is a failure. + */ void sampleGrasps(); /** - * @brief Point cloud call back - * @param msg - point cloud message - * @details Segments objects from table plane - */ + * @brief Point cloud call back + * @param msg - point cloud message + * @details Segments objects from table plane + */ void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); private: @@ -120,10 +120,9 @@ class GraspDetection ros::Subscriber cloud_sub_; // subscribes to point cloud ros::Publisher cloud_pub_; // publishes segmented cloud - std::unique_ptr> - server_; // action server - moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_; // action feedback message - moveit_task_constructor_msgs::SampleGraspPosesResult result_; // action result message + std::unique_ptr> server_; // action server + grasping_msgs::GraspPlanningFeedback feedback_; // action feedback message + grasping_msgs::GraspPlanningResult result_; // action result message std::string path_to_pcd_file_; // path to cylinder pcd file std::string path_to_gpd_config_; // path to GPD config file diff --git a/moveit_task_constructor_gpd/package.xml b/moveit_task_constructor_gpd/package.xml index ae809b3..aebf427 100644 --- a/moveit_task_constructor_gpd/package.xml +++ b/moveit_task_constructor_gpd/package.xml @@ -16,10 +16,13 @@ actionlib eigen_conversions geometry_msgs - moveit_task_constructor_msgs + grasping_msgs rosparam_shortcuts sensor_msgs std_msgs + moveit_msgs + + deep_grasp_task message_runtime diff --git a/moveit_task_constructor_gpd/src/grasp_detection.cpp b/moveit_task_constructor_gpd/src/grasp_detection.cpp index 53f6630..c9de572 100644 --- a/moveit_task_constructor_gpd/src/grasp_detection.cpp +++ b/moveit_task_constructor_gpd/src/grasp_detection.cpp @@ -40,6 +40,7 @@ #include #include #include +#include // Eigen #include @@ -83,8 +84,7 @@ void GraspDetection::loadParameters() void GraspDetection::init() { // action server - server_.reset(new actionlib::SimpleActionServer( - nh_, action_name_, false)); + server_.reset(new actionlib::SimpleActionServer(nh_, action_name_, false)); server_->registerGoalCallback(std::bind(&GraspDetection::goalCallback, this)); server_->registerPreemptCallback(std::bind(&GraspDetection::preemptCallback, this)); server_->start(); @@ -110,7 +110,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()); goal_active_ = true; @@ -147,7 +147,6 @@ void GraspDetection::sampleGrasps() if (grasp_id.empty()) { ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found with a positive cost"); - result_.grasp_state = "failed"; server_->setAborted(result_); return; } @@ -174,15 +173,13 @@ void GraspDetection::sampleGrasps() grasp_pose.pose.orientation.y = rot.y(); grasp_pose.pose.orientation.z = rot.z(); - feedback_.grasp_candidates.emplace_back(grasp_pose); - - // Grasp is selected based on cost not score - // Invert score to represent grasp with lowest cost - feedback_.costs.emplace_back(static_cast(1.0 / grasps.at(id)->getScore())); + moveit_msgs::Grasp current_grasp; + current_grasp.grasp_pose = grasp_pose; + current_grasp.grasp_quality = static_cast(1.0 / grasps.at(id)->getScore()); + feedback_.grasps.emplace_back(current_grasp); } server_->publishFeedback(feedback_); - result_.grasp_state = "success"; server_->setSucceeded(result_); }