added rviz visualization

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})
 
 #############

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_;
   };
 
   /**

include/agile_grasp/handle_search.h

@@ -38,8 +38,11 @@
 #include <pcl/point_cloud.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
-#include <agile_grasp/handle.h>
+#include <ros/ros.h>
+
 #include <agile_grasp/grasp_hypothesis.h>
+#include <agile_grasp/handle.h>
+#include <agile_grasp/plot.h>
 
 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
 
@@ -54,7 +57,7 @@ typedef pcl::PointCloud<pcl::PointXYZ> 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<Handle> findHandles(const std::vector<GraspHypothesis>& 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>& handle_list, const PointCloud::Ptr& cloud, std::string str);
 
 private:
 

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<Handle> findHandles(const std::vector<GraspHypothesis>& hand_list, int min_inliers,
-			double min_length, bool is_plotting = false);
+	std::vector<Handle> findHandles(const std::vector<GraspHypothesis>& 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

include/agile_grasp/plot.h

@@ -34,7 +34,12 @@
 
 #include <pcl/visualization/pcl_visualizer.h>
 
+#include <geometry_msgs/Point.h>
+#include <ros/ros.h>
+#include <visualization_msgs/MarkerArray.h>
+
 #include <agile_grasp/grasp_hypothesis.h>
+#include <agile_grasp/handle.h>
 #include <agile_grasp/quadric.h>
 
 
@@ -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<GraspHypothesis>& 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>& 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>& 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<pcl::visualization::PCLVisualizer> 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 */ 

launch/baxter_grasps.launch

@@ -3,7 +3,7 @@
   	<param name="cloud_topic" value="/register_clouds/point_cloud_sized" />
     <param name="cloud_frame" value="/base" />
     <param name="cloud_type" value="1" />
-    <param name="svm_file_name" value="/home/baxter/baxter_ws/src/grasp_affordances_cpp/svm_032015_20_20_same" />
+    <param name="svm_file_name" value="$(find agile_grasp)/svm_032015_20_20_same" />
     <param name="num_samples" value="8000" />
     <param name="num_threads" value="4" />
     <param name="num_clouds" value="1" />

launch/single_camera_grasps.launch

@@ -3,16 +3,17 @@
   	<param name="cloud_topic" value="/camera/depth_registered/points" />
     <param name="cloud_frame" value="/camera_rgb_optical_frame" />
     <param name="cloud_type" value="0" />
-    <param name="svm_file_name" value="/home/andreas/baxter_ws/src/grasp_affordances_cpp/svm_032015_20_20_same" />
+    <param name="svm_file_name" value="$(find agile_grasp)/svm_032015_20_20_same" />
     <param name="num_samples" value="4000" />
     <param name="num_threads" value="4" />
     <param name="num_clouds" value="1" />
-    <rosparam param="workspace"> [-10, 10, -10, 10, -10, 10] </rosparam>
+		<rosparam param="workspace"> [-10, 10, -10, 10, -10, 10] </rosparam>
     <rosparam param="camera_pose"> [0, 0.445417, 0.895323, 0.215, 
               1, 0, 0, -0.015, 
               0, 0.895323, -0.445417, 0.23, 
               0, 0, 0, 1] </rosparam>
     <param name="min_inliers" value="10" />
-    <param name="plots" value="true" />
+    <param name="plotting" value="2" />
+    <param name="marker_lifetime" value="5" />
 	</node>
 </launch>