diff --git a/CMakeLists.txt b/CMakeLists.txt index d9a2680..bbd30bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -152,8 +152,8 @@ target_link_libraries(opencv_test ${OpenCV_LIBRARIES}) target_link_libraries(handle rotating_hand) target_link_libraries(find_grasps grasp_localizer ${catkin_LIBRARIES} ${PCL_LIBRARIES}) target_link_libraries(grasp_localizer handle localization rotating_hand ${catkin_LIBRARIES} ${PCL_LIBRARIES}) -target_link_libraries(handle_search handle rotating_hand ${PCL_LIBRARIES}) -target_link_libraries(plot grasp_hypothesis quadric ${PCL_LIBRARIES}) +target_link_libraries(handle_search handle plot rotating_hand ${catkin_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(plot grasp_hypothesis handle quadric ${catkin_LIBRARIES} ${PCL_LIBRARIES}) target_link_libraries(hand_search finger_hand grasp_hypothesis plot quadric rotating_hand ${PCL_LIBRARIES}) ############# diff --git a/include/agile_grasp/grasp_localizer.h b/include/agile_grasp/grasp_localizer.h index 1a7cacb..bf94bde 100644 --- a/include/agile_grasp/grasp_localizer.h +++ b/include/agile_grasp/grasp_localizer.h @@ -89,10 +89,13 @@ class GraspLocalizer double hand_depth_; double hand_height_; double init_bite_; - bool plots_hands_; - + // handle search parameters int min_inliers_; + + // visualization parameters + int plotting_mode_; + double marker_lifetime_; }; /** diff --git a/include/agile_grasp/handle_search.h b/include/agile_grasp/handle_search.h index 9241f49..feee8ee 100644 --- a/include/agile_grasp/handle_search.h +++ b/include/agile_grasp/handle_search.h @@ -38,8 +38,11 @@ #include #include -#include +#include + #include +#include +#include typedef pcl::PointCloud PointCloud; @@ -54,7 +57,7 @@ typedef pcl::PointCloud PointCloud; class HandleSearch { public: - + /** * \brief Search for handles given a list of grasp hypotheses. * \param hand_list the list of grasp hypotheses @@ -64,13 +67,6 @@ class HandleSearch */ std::vector findHandles(const std::vector& hand_list, int min_inliers, double min_length); - /** - * \brief Plot a list of handles. - * \param handle_list the list of handles to be plotted - * \param cloud the cloud to be plotted - * \param str the title of the plotting window - */ - void plotHandles(const std::vector& handle_list, const PointCloud::Ptr& cloud, std::string str); private: diff --git a/include/agile_grasp/localization.h b/include/agile_grasp/localization.h index 0803096..9f4d7f3 100644 --- a/include/agile_grasp/localization.h +++ b/include/agile_grasp/localization.h @@ -72,8 +72,7 @@ class Localization /** * \brief Default Constructor. */ - Localization() : num_threads_(1), plots_hands_(true), - plots_camera_sources_(false), cloud_(new PointCloud) + Localization() : num_threads_(1), plotting_mode_(1), plots_camera_sources_(false), cloud_(new PointCloud) { } /** @@ -82,9 +81,9 @@ class Localization * \param filter_boundaries whether grasp hypotheses that are close to the point cloud boundaries are filtered out * \param plots_hands whether grasp hypotheses are plotted */ - Localization(int num_threads, bool filters_boundaries, bool plots_hands) : + Localization(int num_threads, bool filters_boundaries, int plotting_mode) : num_threads_(num_threads), filters_boundaries_(filters_boundaries), - plots_hands_(plots_hands), plots_camera_sources_(false), + plotting_mode_(plotting_mode), plots_camera_sources_(false), cloud_(new PointCloud) { } @@ -95,8 +94,7 @@ class Localization * \param min_length the minimum length of the handle * \param is_plotting whether the handles are plotted */ - std::vector findHandles(const std::vector& hand_list, int min_inliers, - double min_length, bool is_plotting = false); + std::vector findHandles(const std::vector& hand_list, int min_inliers, double min_length); /** * \brief Predict antipodal grasps given a list of grasp hypotheses. @@ -246,6 +244,20 @@ class Localization { hand_height_ = hand_height; } + + /** + * \brief Set the publisher for Rviz visualization, the lifetime of visual markers, and the frame associated with + * the grasps. + * \param node the ROS node + * \param marker_lifetime the lifetime of each visual marker + * \param frame the frame to which the grasps belong + */ + void createVisualsPub(ros::NodeHandle& node, double marker_lifetime, const std::string& frame) + { + plot_.createVisualPublishers(node, marker_lifetime); + visuals_frame_ = frame; + } + private: @@ -324,9 +336,15 @@ class Localization double hand_depth_; ///< hand depth (finger length) double hand_height_; ///< hand height double init_bite_; ///< initial bite - bool plots_camera_sources_; ///< whether the camera source is plotted for each point in the point cloud - bool plots_hands_; ///< whether the results of the grasp hypothesis search are plotted + bool plots_camera_sources_; ///< whether the camera source is plotted for each point in the point cloud bool filters_boundaries_; ///< whether grasp hypotheses close to the workspace boundaries are filtered out + int plotting_mode_; ///< what plotting mode is used + std::string visuals_frame_; ///< visualization frame for Rviz + + /** constants for plotting modes */ + static const int NO_PLOTTING = 0; ///< no plotting + static const int PCL_PLOTTING = 1; ///< plotting in PCL + static const int RVIZ_PLOTTING = 2; ///< plotting in Rviz }; #endif diff --git a/include/agile_grasp/plot.h b/include/agile_grasp/plot.h index b2af867..5dc6623 100644 --- a/include/agile_grasp/plot.h +++ b/include/agile_grasp/plot.h @@ -34,7 +34,12 @@ #include +#include +#include +#include + #include +#include #include @@ -94,6 +99,36 @@ class Plot */ void plotCameraSource(const Eigen::VectorXi& pts_cam_source_in, const PointCloud::Ptr& cloud); + /** + * \brief Create publishers for handles and grasp hypotheses to visualize them in Rviz. + * \param node the ROS node for which the publishers are advertised + * \param marker_lifetime the lifetime of each visual marker + */ + void createVisualPublishers(ros::NodeHandle& node, double marker_lifetime); + + /** + * \brief Plot the grasp hypotheseses in Rviz. + * \param hand_list the list of grasp hypotheses + * \param frame the frame that the poses of the grasp hypotheses are relative to + */ + void plotGraspsRviz(const std::vector& hand_list, const std::string& frame, + bool is_antipodal = false); + + /** + * \brief Plot the handles in Rviz. + * \param handle_list the list of handles + * \param frame the frame that the poses of the handle grasps are relative to + */ + void plotHandlesRviz(const std::vector& handle_list, const std::string& frame); + + /** + * \brief Plot the handles. + * \param handle_list the list of handles + * \param frame the frame that the poses of the handle grasps are relative to + * \param str the title of the PCL visualization window + */ + void plotHandles(const std::vector& handle_list, const PointCloud::Ptr& cloud, std::string str); + private: /** @@ -131,9 +166,8 @@ class Plot * \param str the title of the visualization window * \param use_grasp_bottom whether the grasps plotted originate from the grasp bottom point */ - void plotHandsHelper(const PointCloudNormal::Ptr& hands_cloud, - const PointCloudNormal::Ptr& antipodal_hands_cloud, const PointCloud::Ptr& cloud, - std::string str, bool use_grasp_bottom); + void plotHandsHelper(const PointCloudNormal::Ptr& hands_cloud, const PointCloudNormal::Ptr& antipodal_hands_cloud, + const PointCloud::Ptr& cloud, std::string str, bool use_grasp_bottom); /** * \brief Run/show a PCL visualizer until an escape key is hit. @@ -146,6 +180,30 @@ class Plot * \param title the title of the visualization window */ boost::shared_ptr createViewer(std::string title); + + /** + * \brief Create a visual marker for an approach vector in Rviz. + * \param frame the frame that the marker's pose is relative to + * \param center the position of the marker + * \param approach the approach vector + * \param id the identifier of the marker + * \param color a 3-element array defining the markers color in RGB + * \param alpha the transparency level of the marker + * \param diam the diameter of the marker + */ + visualization_msgs::Marker createApproachMarker(const std::string& frame, const geometry_msgs::Point& center, + const Eigen::Vector3d& approach, int id, const double* color, double alpha, double diam); + + /** + * Create a visual marker. + * \param frame the frame that the marker's pose is relative to + */ + visualization_msgs::Marker createMarker(const std::string& frame); + + ros::Publisher hypotheses_pub_; ///< ROS publisher for grasp hypotheses (Rviz) + ros::Publisher antipodal_pub_; ///< ROS publisher for antipodal grasps (Rviz) + ros::Publisher handles_pub_; ///< ROS publisher for handles (Rviz) + double marker_lifetime_; ///< max time that markers are visualized in Rviz }; #endif /* PLOT_H */ diff --git a/launch/baxter_grasps.launch b/launch/baxter_grasps.launch index ddd6877..afc786d 100644 --- a/launch/baxter_grasps.launch +++ b/launch/baxter_grasps.launch @@ -3,7 +3,7 @@ - + diff --git a/launch/single_camera_grasps.launch b/launch/single_camera_grasps.launch index 61a48cf..d8382f3 100644 --- a/launch/single_camera_grasps.launch +++ b/launch/single_camera_grasps.launch @@ -3,16 +3,17 @@ - + - [-10, 10, -10, 10, -10, 10] + [-10, 10, -10, 10, -10, 10] [0, 0.445417, 0.895323, 0.215, 1, 0, 0, -0.015, 0, 0.895323, -0.445417, 0.23, 0, 0, 0, 1] - + + diff --git a/readme/pcl_vis.png b/readme/pcl_vis.png new file mode 100644 index 0000000..5555e67 Binary files /dev/null and b/readme/pcl_vis.png differ diff --git a/readme/rviz_online1.png b/readme/rviz_online1.png new file mode 100644 index 0000000..332abff Binary files /dev/null and b/readme/rviz_online1.png differ diff --git a/readme/rviz_online2.png b/readme/rviz_online2.png new file mode 100644 index 0000000..b638229 Binary files /dev/null and b/readme/rviz_online2.png differ diff --git a/readme/rviz_online3.png b/readme/rviz_online3.png new file mode 100644 index 0000000..43ef71a Binary files /dev/null and b/readme/rviz_online3.png differ diff --git a/rviz/single_camera.rviz b/rviz/single_camera.rviz new file mode 100644 index 0000000..bd2cb85 --- /dev/null +++ b/rviz/single_camera.rviz @@ -0,0 +1,166 @@ +Panels: + - Class: rviz/Displays + Help Height: 81 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /Grasp Hypotheses1 + - /Antipodal Grasps1 + - /Handles1 + Splitter Ratio: 0.5 + Tree Height: 749 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 170; 0 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /camera/depth_registered/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /find_grasps/grasp_hypotheses_visual + Name: Grasp Hypotheses + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /find_grasps/antipodal_grasps_visual + Name: Antipodal Grasps + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /find_grasps/handles_visual + Name: Handles + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: camera_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.13516 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.839582 + Y: -0.0704971 + Z: -0.162067 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.589796 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.08046 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1058 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001990000037ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000037f000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000410000037f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000037f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/src/agile_grasp/grasp_localizer.cpp b/src/agile_grasp/grasp_localizer.cpp index 0588b85..6db607b 100644 --- a/src/agile_grasp/grasp_localizer.cpp +++ b/src/agile_grasp/grasp_localizer.cpp @@ -6,7 +6,7 @@ GraspLocalizer::GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_t const Parameters& params) : cloud_left_(new PointCloud()), cloud_right_(new PointCloud()), cloud_frame_(cloud_frame), svm_file_name_(svm_file_name), num_clouds_(params.num_clouds_), - num_clouds_received_(0), plots_handles_(params.plots_hands_) + num_clouds_received_(0), size_left_(0) { // subscribe to input point cloud ROS topic if (cloud_type == CLOUD_SIZED) @@ -18,9 +18,8 @@ GraspLocalizer::GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_t grasps_pub_ = node.advertise("grasps", 10); // create localization object and initialize its parameters - localization_ = new Localization(params.num_threads_, true, params.plots_hands_); - localization_->setCameraTransforms(params.cam_tf_left_, - params.cam_tf_right_); + localization_ = new Localization(params.num_threads_, true, params.plotting_mode_); + localization_->setCameraTransforms(params.cam_tf_left_, params.cam_tf_right_); localization_->setWorkspace(params.workspace_); localization_->setNumSamples(params.num_samples_); localization_->setFingerWidth(params.finger_width_); @@ -28,8 +27,19 @@ GraspLocalizer::GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_t localization_->setHandDepth(params.hand_depth_); localization_->setInitBite(params.init_bite_); localization_->setHandHeight(params.hand_height_); - + min_inliers_ = params.min_inliers_; + + if (params.plotting_mode_ == 0) + { + plots_handles_ = false; + } + else + { + plots_handles_ = false; + if (params.plotting_mode_ == 2) + localization_->createVisualsPub(node, params.marker_lifetime_, cloud_frame_); + } } @@ -42,8 +52,8 @@ void GraspLocalizer::cloud_callback(const sensor_msgs::PointCloud2ConstPtr& msg) if (cloud_frame_.compare(msg->header.frame_id) != 0 && cloud_frame_.compare("/" + msg->header.frame_id) != 0) { - std::cout << "Input cloud frame " << msg->header.frame_id << - " is not equal to parameter " << cloud_frame_ << std::endl; + std::cout << "Input cloud frame " << msg->header.frame_id << " is not equal to parameter " << cloud_frame_ + << std::endl; std::exit(EXIT_FAILURE); } @@ -91,10 +101,12 @@ void GraspLocalizer::localizeGrasps() hands_ = localization_->localizeHands(cloud, cloud_left_->size(), indices, false, false); } else - hands_ = localization_->localizeHands(cloud_left_, size_left_, indices, false, false); + { + hands_ = localization_->localizeHands(cloud_left_, cloud_left_->size(), indices, false, false); + } antipodal_hands_ = localization_->predictAntipodalHands(hands_, svm_file_name_); - handles_ = localization_->findHandles(antipodal_hands_, min_inliers_, 0.005, plots_handles_); + handles_ = localization_->findHandles(antipodal_hands_, min_inliers_, 0.005); // publish handles grasps_pub_.publish(createGraspsMsg(handles_)); diff --git a/src/agile_grasp/handle_search.cpp b/src/agile_grasp/handle_search.cpp index 0c4c351..c105e04 100644 --- a/src/agile_grasp/handle_search.cpp +++ b/src/agile_grasp/handle_search.cpp @@ -117,114 +117,6 @@ bool HandleSearch::shortenHandle(std::vector &inliers, double g return is_done; } -void HandleSearch::plotHandles(const std::vector& handle_list, const PointCloud::Ptr& cloud, - std::string str) -{ - std::cout << "Drawing " << handle_list.size() << " handles.\n"; - - double colors[9][3] = { { 0, 0.4470, 0.7410 }, { 0.8500, 0.3250, 0.0980 }, { 0.9290, 0.6940, 0.1250 }, { - 0.4940, 0.1840, 0.5560 }, { 0.4660, 0.6740, 0.1880 }, { 0.3010, 0.7450, 0.9330 }, { 0.6350, 0.0780, - 0.1840 }, { 0, 0.4470, 0.7410 }, { 0.8500, 0.3250, 0.0980 } }; - - // 0.9290 0.6940 0.1250 - // 0.4940 0.1840 0.5560 - // 0.4660 0.6740 0.1880 - // 0.3010 0.7450 0.9330 - // 0.6350 0.0780 0.1840 - // 0 0.4470 0.7410 - // 0.8500 0.3250 0.0980 - // 0.9290 0.6940 0.1250 - // 0.4940 0.1840 0.5560 - // 0.4660 0.6740 0.1880 - // 0.3010 0.7450 0.9330 - // 0.6350 0.0780 0.1840 - - std::vector::Ptr> clouds; - pcl::PointCloud::Ptr handle_cloud(new pcl::PointCloud()); - - for (int i = 0; i < handle_list.size(); i++) - { - pcl::PointNormal p; - p.x = handle_list[i].getHandsCenter()(0); - p.y = handle_list[i].getHandsCenter()(1); - p.z = handle_list[i].getHandsCenter()(2); - p.normal[0] = -handle_list[i].getApproach()(0); - p.normal[1] = -handle_list[i].getApproach()(1); - p.normal[2] = -handle_list[i].getApproach()(2); - handle_cloud->points.push_back(p); - - const std::vector& inliers = handle_list[i].getInliers(); - const std::vector& hand_list = handle_list[i].getHandList(); - pcl::PointCloud::Ptr axis_cloud(new pcl::PointCloud); - - for (int j = 0; j < inliers.size(); j++) - { - pcl::PointNormal p; - p.x = hand_list[inliers[j]].getGraspSurface()(0); - p.y = hand_list[inliers[j]].getGraspSurface()(1); - p.z = hand_list[inliers[j]].getGraspSurface()(2); - p.normal[0] = -hand_list[inliers[j]].getApproach()(0); - p.normal[1] = -hand_list[inliers[j]].getApproach()(1); - p.normal[2] = -hand_list[inliers[j]].getApproach()(2); - axis_cloud->points.push_back(p); - } - clouds.push_back(axis_cloud); - } - - std::string title = "Handles: " + str; - boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer(title)); - viewer->setBackgroundColor(1, 1, 1); - //viewer->setPosition(0, 0); - //viewer->setSize(640, 480); - viewer->addPointCloud(cloud, "registered point cloud"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, - "registered point cloud"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, - "registered point cloud"); - - for (int i = 0; i < clouds.size(); i++) - { - std::string name = "handle_" + boost::lexical_cast(i); - int ci = i % 6; -// std::cout << "ci: " << ci << "\n"; - viewer->addPointCloud(clouds[i], name); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[ci][0], - colors[ci][1], colors[ci][2], name); - std::string name2 = "approach_" + boost::lexical_cast(i); - viewer->addPointCloudNormals(clouds[i], 1, 0.04, name2); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, name2); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[ci][0], - colors[ci][1], colors[ci][2], name2); - } - - viewer->addPointCloud(handle_cloud, "handles"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "handles"); - viewer->addPointCloudNormals(handle_cloud, 1, 0.08, "approach"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 4, "approach"); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "approach"); - - // viewer->addCoordinateSystem(1.0, "", 0); - viewer->initCameraParameters(); - viewer->setPosition(0, 0); - viewer->setSize(640, 480); - - while (!viewer->wasStopped()) - { - viewer->spinOnce(100); - boost::this_thread::sleep(boost::posix_time::microseconds(100000)); - } - - viewer->close(); - - // std::vector cam; - // - // // print the position of the camera - // viewer->getCameras(cam); - // std::cout << "Cam: " << std::endl << " - pos: (" << cam[0].pos[0] << ", " << cam[0].pos[1] << ", " - // << cam[0].pos[2] << ")" << std::endl << " - view: (" << cam[0].view[0] << ", " << cam[0].view[1] << ", " - // << cam[0].view[2] << ")" << std::endl << " - focal: (" << cam[0].focal[0] << ", " << cam[0].focal[1] - // << ", " << cam[0].focal[2] << ")" << std::endl; -} double HandleSearch::safeAcos(double x) { diff --git a/src/agile_grasp/localization.cpp b/src/agile_grasp/localization.cpp index 8076773..e3b2e46 100644 --- a/src/agile_grasp/localization.cpp +++ b/src/agile_grasp/localization.cpp @@ -2,10 +2,18 @@ std::vector Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left, const std::vector& indices, bool calculates_antipodal, bool uses_clustering) -{ +{ double t0 = omp_get_wtime(); std::vector hand_list; - + + if (size_left == 0 || cloud_in->size() == 0) + { + std::cout << "Input cloud is empty!\n"; + std::cout << size_left << std::endl; + hand_list.resize(0); + return hand_list; + } + // set camera source for all points (0 = left, 1 = right) std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n"; Eigen::VectorXi pts_cam_source(cloud_in->size()); @@ -92,11 +100,17 @@ std::vector Localization::localizeHands(const PointCloud::Ptr& // draw down-sampled and workspace reduced cloud cloud_plot = cloud; - // find hand configurations - HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, - hand_height_, init_bite_, num_threads_, num_samples_, plots_hands_); - hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, - calculates_antipodal, uses_clustering); + // set plotting within handle search on/off + bool plots_hands; + if (plotting_mode_ == PCL_PLOTTING) + plots_hands = true; + else + plots_hands = false; + + // find hand configurations + HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, hand_height_, init_bite_, num_threads_, + num_samples_, plots_hands); + hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, calculates_antipodal, uses_clustering); // remove hands at boundaries of workspace if (filters_boundaries_) @@ -109,14 +123,20 @@ std::vector Localization::localizeHands(const PointCloud::Ptr& double t2 = omp_get_wtime(); std::cout << "Hand localization done in " << t2 - t0 << " sec\n"; - if (plots_hands_) + if (plotting_mode_ == PCL_PLOTTING) + { plot_.plotHands(hand_list, cloud_plot, ""); + } + else if (plotting_mode_ == RVIZ_PLOTTING) + { + plot_.plotGraspsRviz(hand_list, visuals_frame_); + } return hand_list; } -std::vector Localization::predictAntipodalHands( - const std::vector& hand_list, const std::string& svm_filename) +std::vector Localization::predictAntipodalHands(const std::vector& hand_list, + const std::string& svm_filename) { double t0 = omp_get_wtime(); std::vector antipodal_hands; @@ -127,8 +147,10 @@ std::vector Localization::predictAntipodalHands( antipodal_hands = learn.classify(hand_list, svm_filename, cams_mat); std::cout << " runtime: " << omp_get_wtime() - t0 << " sec\n"; std::cout << antipodal_hands.size() << " antipodal hand configurations found\n"; - if (plots_hands_) + if (plotting_mode_ == PCL_PLOTTING) plot_.plotHands(hand_list, antipodal_hands, cloud_, "Antipodal Hands"); + else if (plotting_mode_ == RVIZ_PLOTTING) + plot_.plotGraspsRviz(antipodal_hands, visuals_frame_, true); return antipodal_hands; } @@ -354,11 +376,13 @@ std::vector Localization::filterHands(const std::vector Localization::findHandles(const std::vector& hand_list, int min_inliers, - double min_length, bool is_plotting) + double min_length) { HandleSearch handle_search; std::vector handles = handle_search.findHandles(hand_list, min_inliers, min_length); - if (is_plotting) - handle_search.plotHandles(handles, cloud_, ""); + if (plotting_mode_ == PCL_PLOTTING) + plot_.plotHandles(handles, cloud_, "Handles"); + else if (plotting_mode_ == RVIZ_PLOTTING) + plot_.plotHandlesRviz(handles, visuals_frame_); return handles; } diff --git a/src/agile_grasp/plot.cpp b/src/agile_grasp/plot.cpp index 41afe88..6a13fac 100644 --- a/src/agile_grasp/plot.cpp +++ b/src/agile_grasp/plot.cpp @@ -197,3 +197,210 @@ boost::shared_ptr Plot::createViewer(std::str viewer->setBackgroundColor(1, 1, 1); return viewer; } + + +void Plot::createVisualPublishers(ros::NodeHandle& node, double marker_lifetime) +{ + hypotheses_pub_ = node.advertise("grasp_hypotheses_visual", 10); + antipodal_pub_ = node.advertise("antipodal_grasps_visual", 10); + handles_pub_ = node.advertise("handles_visual", 10); + marker_lifetime_ = marker_lifetime; +} + +void Plot::plotGraspsRviz(const std::vector& hand_list, const std::string& frame, bool is_antipodal) +{ + double red[3] = {1, 0, 0}; + double cyan[3] = {0, 1, 1}; + double* color; + if (is_antipodal) + { + color = red; + std::cout << "Visualizing antipodal grasps in Rviz ...\n"; + } + else + { + color = cyan; + std::cout << "Visualizing grasp hypotheses in Rviz ...\n"; + } + + visualization_msgs::MarkerArray marker_array; + marker_array.markers.resize(hand_list.size()); + + for (int i=0; i < hand_list.size(); i++) + { + geometry_msgs::Point position; + position.x = hand_list[i].getGraspSurface()(0); + position.y = hand_list[i].getGraspSurface()(1); + position.z = hand_list[i].getGraspSurface()(2); + visualization_msgs::Marker marker = createApproachMarker(frame, position, hand_list[i].getApproach(), i, color, 0.4, + 0.004); + marker.ns = "grasp_hypotheses"; + marker.id = i; + marker_array.markers[i] = marker; + } + + if (is_antipodal) + antipodal_pub_.publish(marker_array); + else + hypotheses_pub_.publish(marker_array); + + ros::Duration(1.0).sleep(); +} + + +void Plot::plotHandlesRviz(const std::vector& handle_list, const std::string& frame) +{ + std::cout << "Visualizing handles in Rviz ...\n"; + double green[3] = {0, 1, 0}; + visualization_msgs::MarkerArray marker_array; + marker_array.markers.resize(handle_list.size()); + + for (int i=0; i < handle_list.size(); i++) + { + geometry_msgs::Point position; + position.x = handle_list[i].getHandsCenter()(0); + position.y = handle_list[i].getHandsCenter()(1); + position.z = handle_list[i].getHandsCenter()(2); + visualization_msgs::Marker marker = createApproachMarker(frame, position, handle_list[i].getApproach(), i, green, 0.6, + 0.008); + marker.ns = "handles"; + marker_array.markers[i] = marker; + } + + handles_pub_.publish(marker_array); + ros::Duration(1.0).sleep(); +} + + +void Plot::plotHandles(const std::vector& handle_list, const PointCloud::Ptr& cloud, std::string str) +{ + double colors[10][3] = { { 0, 0.4470, 0.7410 }, { 0.8500, 0.3250, 0.0980 }, { 0.9290, 0.6940, 0.1250 }, { + 0.4940, 0.1840, 0.5560 }, { 0.4660, 0.6740, 0.1880 }, { 0.3010, 0.7450, 0.9330 }, { 0.6350, 0.0780, + 0.1840 }, { 0, 0.4470, 0.7410 }, { 0.8500, 0.3250, 0.0980 }, { 0.9290, 0.6940, 0.1250} }; + + // 0.4940 0.1840 0.5560 + // 0.4660 0.6740 0.1880 + // 0.3010 0.7450 0.9330 + // 0.6350 0.0780 0.1840 + // 0 0.4470 0.7410 + // 0.8500 0.3250 0.0980 + // 0.9290 0.6940 0.1250 + // 0.4940 0.1840 0.5560 + // 0.4660 0.6740 0.1880 + // 0.3010 0.7450 0.9330 + // 0.6350 0.0780 0.1840 + + std::vector::Ptr> clouds; + pcl::PointCloud::Ptr handle_cloud(new pcl::PointCloud()); + + for (int i = 0; i < handle_list.size(); i++) + { + pcl::PointNormal p; + p.x = handle_list[i].getHandsCenter()(0); + p.y = handle_list[i].getHandsCenter()(1); + p.z = handle_list[i].getHandsCenter()(2); + p.normal[0] = -handle_list[i].getApproach()(0); + p.normal[1] = -handle_list[i].getApproach()(1); + p.normal[2] = -handle_list[i].getApproach()(2); + handle_cloud->points.push_back(p); + + const std::vector& inliers = handle_list[i].getInliers(); + const std::vector& hand_list = handle_list[i].getHandList(); + pcl::PointCloud::Ptr axis_cloud(new pcl::PointCloud); + + for (int j = 0; j < inliers.size(); j++) + { + pcl::PointNormal p; + p.x = hand_list[inliers[j]].getGraspSurface()(0); + p.y = hand_list[inliers[j]].getGraspSurface()(1); + p.z = hand_list[inliers[j]].getGraspSurface()(2); + p.normal[0] = -hand_list[inliers[j]].getApproach()(0); + p.normal[1] = -hand_list[inliers[j]].getApproach()(1); + p.normal[2] = -hand_list[inliers[j]].getApproach()(2); + axis_cloud->points.push_back(p); + } + clouds.push_back(axis_cloud); + } + + std::string title = "Handles: " + str; + boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer(title)); + viewer->setBackgroundColor(1, 1, 1); + //viewer->setPosition(0, 0); + //viewer->setSize(640, 480); + viewer->addPointCloud(cloud, "registered point cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "registered point cloud"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, + "registered point cloud"); + + for (int i = 0; i < clouds.size(); i++) + { + std::string name = "handle_" + boost::lexical_cast(i); + int ci = i % 6; +// std::cout << "ci: " << ci << "\n"; + viewer->addPointCloud(clouds[i], name); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[ci][0], + colors[ci][1], colors[ci][2], name); + std::string name2 = "approach_" + boost::lexical_cast(i); + viewer->addPointCloudNormals(clouds[i], 1, 0.04, name2); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, name2); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[ci][0], + colors[ci][1], colors[ci][2], name2); + } + + viewer->addPointCloud(handle_cloud, "handles"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "handles"); + viewer->addPointCloudNormals(handle_cloud, 1, 0.08, "approach"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 4, "approach"); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "approach"); + + // viewer->addCoordinateSystem(1.0, "", 0); + viewer->initCameraParameters(); + viewer->setPosition(0, 0); + viewer->setSize(640, 480); + + while (!viewer->wasStopped()) + { + viewer->spinOnce(100); + boost::this_thread::sleep(boost::posix_time::microseconds(100000)); + } + + viewer->close(); +} + + +visualization_msgs::Marker Plot::createApproachMarker(const std::string& frame, const geometry_msgs::Point& center, + const Eigen::Vector3d& approach, int id, const double* color, double alpha, double diam) +{ + visualization_msgs::Marker marker = createMarker(frame); + marker.type = visualization_msgs::Marker::ARROW; + marker.id = id; + marker.scale.x = diam; // shaft diameter + marker.scale.y = diam; // head diameter + marker.scale.z = 0.01; // head length + marker.color.r = color[0]; + marker.color.g = color[1]; + marker.color.b = color[2]; + marker.color.a = alpha; + geometry_msgs::Point p, q; + p.x = center.x; + p.y = center.y; + p.z = center.z; + q.x = p.x - 0.03 * approach(0); + q.y = p.y - 0.03 * approach(1); + q.z = p.z - 0.03 * approach(2); + marker.points.push_back(p); + marker.points.push_back(q); + return marker; +} + + +visualization_msgs::Marker Plot::createMarker(const std::string& frame) +{ + visualization_msgs::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = ros::Time::now(); + marker.lifetime = ros::Duration(marker_lifetime_); + marker.action = visualization_msgs::Marker::ADD; + return marker; +} diff --git a/src/nodes/find_grasps.cpp b/src/nodes/find_grasps.cpp index 995ee58..60a3e7b 100644 --- a/src/nodes/find_grasps.cpp +++ b/src/nodes/find_grasps.cpp @@ -19,6 +19,7 @@ const double WORKSPACE[6] = {0.65, 0.9, -0.2, 0.07, -0.3, 1.0}; const int MIN_HANDLE_INLIERS = 3; const int CLOUD_TYPE = 0; const std::string CLOUD_TYPES[2] = {"sensor_msgs/PointCloud2", "grasp_affordances/CloudSized"}; +const std::string PLOT_MODES[3] = {"none", "pcl", "rviz"}; int main(int argc, char** argv) @@ -64,8 +65,9 @@ int main(int argc, char** argv) node.param("min_inliers", params.min_inliers_, MIN_HANDLE_INLIERS); node.getParam("workspace", workspace); node.getParam("camera_pose", camera_pose); - node.param("plots", params.plots_hands_, false); - + node.param("plotting", params.plotting_mode_, 0); + node.param("marker_lifetime", params.marker_lifetime_, 0.0); + Eigen::Matrix4d R; for (int i=0; i < R.rows(); i++) R.row(i) << camera_pose[i*R.cols()], camera_pose[i*R.cols() + 1], camera_pose[i*R.cols() + 2], camera_pose[i*R.cols() + 3]; @@ -83,8 +85,7 @@ int main(int argc, char** argv) std::cout << " workspace: " << ws.transpose() << "\n"; std::cout << " num_samples: " << params.num_samples_ << "\n"; std::cout << " num_threads: " << params.num_threads_ << "\n"; - std::cout << " num_clouds: " << params.num_clouds_ << "\n"; - std::cout << " plots: " << params.plots_hands_ << "\n"; + std::cout << " num_clouds: " << params.num_clouds_ << "\n"; std::cout << " camera pose:\n" << R << std::endl; std::cout << " Robot Hand Model\n"; std::cout << " finger_width: " << params.finger_width_ << "\n"; @@ -96,6 +97,9 @@ int main(int argc, char** argv) std::cout << " svm_file_name: " << svm_file_name << "\n"; std::cout << " Handle Search\n"; std::cout << " min_inliers: " << params.min_inliers_ << "\n"; + std::cout << " Visualization\n"; + std::cout << " plot_mode: " << PLOT_MODES[params.plotting_mode_] << "\n"; + std::cout << " marker_lifetime: " << params.marker_lifetime_ << "\n"; GraspLocalizer loc(node, cloud_topic, cloud_frame, cloud_type, svm_file_name, params); loc.localizeGrasps(); diff --git a/src/nodes/test.cpp b/src/nodes/test.cpp index de43d17..c98c265 100644 --- a/src/nodes/test.cpp +++ b/src/nodes/test.cpp @@ -61,10 +61,10 @@ int main(int argc, char** argv) //workspace << 0.55, 0.9, -0.35, 0.2, -0.2, 2; //workspace << 0.6, 0.8, -0.25, 0.1, -0.3, 2; // workspace << 0.55, 0.95, -0.25, 0.07, -0.3, 1; - // workspace << -10, 10, -10, 10, -10, 10; - workspace << -10, 10, -10, 10, 0.55, 0.95; + workspace << -10, 10, -10, 10, -10, 10; + // workspace << -10, 10, -10, 10, 0.55, 0.95; - // set-up parameters for the hand search + // set-up parameters for the hand search Localization loc(num_threads, true, true); loc.setCameraTransforms(base_tf * sqrt_tf.inverse(), base_tf * sqrt_tf); loc.setWorkspace(workspace); @@ -90,7 +90,7 @@ int main(int argc, char** argv) // // test with randomly sampled indices std::vector hands = loc.localizeHands(file_name_left, file_name_right); std::vector antipodal_hands = loc.predictAntipodalHands(hands, svm_file_name); - std::vector handles = loc.findHandles(antipodal_hands, min_inliers, 0.005, true); + std::vector handles = loc.findHandles(antipodal_hands, min_inliers, 0.005); return 0; }