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..65ac369 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,10 @@ #include #include #include -#include + +//#include +#include + #include #include #include @@ -64,7 +67,7 @@ #include #include #include -#include +//#include namespace deep_grasp_task { diff --git a/deep_grasp_task/launch/panda_world.launch b/deep_grasp_task/launch/panda_world.launch index f7444c5..3bd8515 100644 --- a/deep_grasp_task/launch/panda_world.launch +++ b/deep_grasp_task/launch/panda_world.launch @@ -26,7 +26,7 @@ respawn="false" output="screen" /> - + diff --git a/deep_grasp_task/src/deep_grasp_demo.cpp b/deep_grasp_task/src/deep_grasp_demo.cpp index ec4a3be..01bfa9b 100644 --- a/deep_grasp_task/src/deep_grasp_demo.cpp +++ b/deep_grasp_task/src/deep_grasp_demo.cpp @@ -49,7 +49,8 @@ #include #include -#include +//#include +#include #include constexpr char LOGNAME[] = "deep_grasp_demo"; @@ -224,7 +225,7 @@ int main(int argc, char** argv) if (deep_pick_place_task.plan()) { ROS_INFO_NAMED(LOGNAME, "Planning succeded"); - if (pnh.param("execute", false)) + if (pnh.param("execute", true)) { deep_pick_place_task.execute(); ROS_INFO_NAMED(LOGNAME, "Execution complete"); diff --git a/deep_grasp_task/src/deep_pick_place_task.cpp b/deep_grasp_task/src/deep_pick_place_task.cpp index b017948..ab44718 100644 --- a/deep_grasp_task/src/deep_pick_place_task.cpp +++ b/deep_grasp_task/src/deep_pick_place_task.cpp @@ -204,8 +204,9 @@ 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"); + auto stage = std::make_unique( action_name_, "generate grasp pose", 0.0, 0.0); stage->properties().configureInitFrom(Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose(hand_open_pose_); diff --git a/moveit_task_constructor_dexnet/config/dexnet_config.yaml b/moveit_task_constructor_dexnet/config/dexnet_config.yaml index b8d1a05..bda1532 100644 --- a/moveit_task_constructor_dexnet/config/dexnet_config.yaml +++ b/moveit_task_constructor_dexnet/config/dexnet_config.yaml @@ -1,4 +1,4 @@ # file path to neural network weights and model config model_name: "GQCNN-4.0-PJ" -model_dir: "/home/bostoncleek/dex-net/deps/gqcnn/models/GQCNN-4.0-PJ" -model_params: "/home/bostoncleek/dex-net/deps/gqcnn/cfg/examples/replication/dex-net_4.0_pj.yaml" +model_dir: "/home/pcl/grasp_ws/src/deep_grasp_demo/dexnet_deps/gqcnn/models/GQCNN-4.0-PJ" +model_params: "/home/pcl/grasp_ws/src/deep_grasp_demo/dexnet_deps/gqcnn/cfg/examples/replication/dex-net_4.0_pj.yaml" 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..bc11833 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,8 @@ #include // Action Server -#include +//#include +#include #include namespace moveit_task_constructor_dexnet @@ -107,10 +108,10 @@ class GraspDetection ros::ServiceClient gqcnn_client_; // gqcnn service client ros::ServiceClient image_client_; // image saving service client - std::unique_ptr> + std::unique_ptr> server_; // action server - moveit_task_constructor_msgs::SampleGraspPosesFeedback feedback_; // action feedback message - moveit_task_constructor_msgs::SampleGraspPosesResult result_; // action result message + 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/src/grasp_detection.cpp b/moveit_task_constructor_dexnet/src/grasp_detection.cpp index ed8553a..f0e5c86 100644 --- a/moveit_task_constructor_dexnet/src/grasp_detection.cpp +++ b/moveit_task_constructor_dexnet/src/grasp_detection.cpp @@ -78,7 +78,9 @@ void GraspDetection::loadParameters() void GraspDetection::init() { // action server - server_.reset(new actionlib::SimpleActionServer( + //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)); @@ -101,7 +103,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 @@ -156,10 +158,12 @@ void GraspDetection::sampleGrasps() const std::vector q_values = grasp_srv.response.q_values; ROS_INFO_NAMED(LOGNAME, "Results recieved"); + moveit_msgs::Grasp current_grasp; + if (grasp_candidates.empty()) { ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found"); - result_.grasp_state = "failed"; + //result_.grasp_state = "failed"; server_->setAborted(result_); return; } @@ -192,6 +196,7 @@ void GraspDetection::sampleGrasps() grasp_pose.pose.orientation.y = rot.y(); grasp_pose.pose.orientation.z = rot.z(); + /* // send feedback to action client feedback_.grasp_candidates.emplace_back(grasp_pose); @@ -200,17 +205,23 @@ void GraspDetection::sampleGrasps() 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_pose = grasp_pose; + const double cost = 1.0 - q_values.at(i); + current_grasp.grasp_quality = cost; + feedback_.grasps.emplace_back(current_grasp); } server_->publishFeedback(feedback_); - result_.grasp_state = "success"; + //result_.grasp_state = "success"; server_->setSucceeded(result_); } else { ROS_ERROR_NAMED(LOGNAME, "Failed to call gqcnn_grasp service"); - result_.grasp_state = "failed"; + //result_.grasp_state = "failed"; server_->setAborted(result_); } } diff --git a/moveit_task_constructor_gpd/config/gpd_config.yaml b/moveit_task_constructor_gpd/config/gpd_config.yaml index a12f17b..5bc5a84 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/pcl/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/grasp_detection.h b/moveit_task_constructor_gpd/include/moveit_task_constructor_gpd/grasp_detection.h index ec98b42..b14e6ba 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,9 @@ #include // Action Server -#include + +//#include +#include #include namespace moveit_task_constructor_gpd @@ -120,10 +122,15 @@ 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 + //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_; + grasping_msgs::GraspPlanningResult result_; + 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/src/grasp_detection.cpp b/moveit_task_constructor_gpd/src/grasp_detection.cpp index 53f6630..0f07c64 100644 --- a/moveit_task_constructor_gpd/src/grasp_detection.cpp +++ b/moveit_task_constructor_gpd/src/grasp_detection.cpp @@ -82,12 +82,21 @@ void GraspDetection::loadParameters() void GraspDetection::init() { + /* // action server 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(); + */ + + // action server + 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(); // GPD point cloud camera, load cylinder from file // set camera view origin @@ -110,7 +119,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; @@ -134,6 +143,8 @@ void GraspDetection::sampleGrasps() grasp_detector_->preprocessPointCloud(*cloud_camera_); // preprocess the point cloud grasps = grasp_detector_->detectGrasps(*cloud_camera_); // detect grasps in the point cloud + moveit_msgs::Grasp current_grasp; + // Use grasps with score > 0 std::vector grasp_id; for (unsigned int i = 0; i < grasps.size(); i++) @@ -147,7 +158,7 @@ void GraspDetection::sampleGrasps() if (grasp_id.empty()) { ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found with a positive cost"); - result_.grasp_state = "failed"; + //result_.grasp_state = "failed"; server_->setAborted(result_); return; } @@ -174,15 +185,21 @@ 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())); + */ + + 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"; + //result_.grasp_state = "success"; server_->setSucceeded(result_); }