obj&grab demo

wpb_home_behaviors/launch/obj_3d.launch

@@ -0,0 +1,112 @@
+<launch>
+ 
+  <!--- Run wpb_home  -->
+  <include file="$(find wpb_home_bringup)/launch/normal.launch" />
+
+  <node pkg="wpb_home_behaviors" type="wpb_home_objects_3d" name="wpb_home_objects_3d" output="screen">
+    <rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
+  </node>
+
+  <node pkg="wpb_home_behaviors" type="wpb_home_grab_action" name="wpb_home_grab_action">
+    <rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
+  </node>
+
+  <!--- Run Kinect -->
+  <!--************************ kinect ************************-->
+  <arg name="base_name"         default="kinect2"/>
+  <arg name="sensor"            default=""/>
+  <arg name="publish_tf"        default="false"/>
+  <arg name="base_name_tf"      default="$(arg base_name)"/>
+  <arg name="fps_limit"         default="-1.0"/>
+  <arg name="calib_path"        default="$(find kinect2_bridge)/data/"/>
+  <arg name="use_png"           default="false"/>
+  <arg name="jpeg_quality"      default="90"/>
+  <arg name="png_level"         default="1"/>
+  <arg name="depth_method"      default="cpu"/><!--- default/cpu -->
+  <arg name="depth_device"      default="-1"/>
+  <arg name="reg_method"        default="cpu"/>
+  <arg name="reg_device"        default="-1"/>
+  <arg name="max_depth"         default="12.0"/>
+  <arg name="min_depth"         default="0.1"/>
+  <arg name="queue_size"        default="5"/>
+  <arg name="bilateral_filter"  default="true"/>
+  <arg name="edge_aware_filter" default="true"/>
+  <arg name="worker_threads"    default="4"/>
+  <arg name="machine"           default="localhost"/>
+  <arg name="nodelet_manager"   default="$(arg base_name)"/>
+  <arg name="start_manager"     default="true"/>
+  <arg name="use_machine"       default="true"/>
+  <arg name="respawn"           default="true"/>
+  <arg name="use_nodelet"       default="true"/>
+  <arg name="output"            default="log"/>
+
+  <machine name="localhost" address="localhost" if="$(arg use_machine)"/>
+
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager"
+        if="$(arg start_manager)" machine="$(arg machine)" />
+
+  <!-- Nodelet version of kinect2_bridge -->
+  <node pkg="nodelet" type="nodelet" name="$(arg base_name)_bridge" machine="$(arg machine)"
+        args="load kinect2_bridge/kinect2_bridge_nodelet $(arg nodelet_manager)"
+        respawn="$(arg respawn)" output="$(arg output)" if="$(arg use_nodelet)">
+    <param name="base_name"         type="str"    value="$(arg base_name)"/>
+    <param name="sensor"            type="str"    value="$(arg sensor)"/>
+    <param name="publish_tf"        type="bool"   value="$(arg publish_tf)"/>
+    <param name="base_name_tf"      type="str"    value="$(arg base_name_tf)"/>
+    <param name="fps_limit"         type="double" value="$(arg fps_limit)"/>
+    <param name="calib_path"        type="str"    value="$(arg calib_path)"/>
+    <param name="use_png"           type="bool"   value="$(arg use_png)"/>
+    <param name="jpeg_quality"      type="int"    value="$(arg jpeg_quality)"/>
+    <param name="png_level"         type="int"    value="$(arg png_level)"/>
+    <param name="depth_method"      type="str"    value="$(arg depth_method)"/>
+    <param name="depth_device"      type="int"    value="$(arg depth_device)"/>
+    <param name="reg_method"        type="str"    value="$(arg reg_method)"/>
+    <param name="reg_device"        type="int"    value="$(arg reg_device)"/>
+    <param name="max_depth"         type="double" value="$(arg max_depth)"/>
+    <param name="min_depth"         type="double" value="$(arg min_depth)"/>
+    <param name="queue_size"        type="int"    value="$(arg queue_size)"/>
+    <param name="bilateral_filter"  type="bool"   value="$(arg bilateral_filter)"/>
+    <param name="edge_aware_filter" type="bool"   value="$(arg edge_aware_filter)"/>
+    <param name="worker_threads"    type="int"    value="$(arg worker_threads)"/>
+  </node>
+
+  <!-- Node version of kinect2_bridge -->
+  <node pkg="kinect2_bridge" type="kinect2_bridge" name="$(arg base_name)_bridge" machine="$(arg machine)"
+        respawn="$(arg respawn)" output="$(arg output)" unless="$(arg use_nodelet)">
+    <param name="base_name"         type="str"    value="$(arg base_name)"/>
+    <param name="sensor"            type="str"    value="$(arg sensor)"/>
+    <param name="publish_tf"        type="bool"   value="$(arg publish_tf)"/>
+    <param name="base_name_tf"      type="str"    value="$(arg base_name_tf)"/>
+    <param name="fps_limit"         type="double" value="$(arg fps_limit)"/>
+    <param name="calib_path"        type="str"    value="$(arg calib_path)"/>
+    <param name="use_png"           type="bool"   value="$(arg use_png)"/>
+    <param name="jpeg_quality"      type="int"    value="$(arg jpeg_quality)"/>
+    <param name="png_level"         type="int"    value="$(arg png_level)"/>
+    <param name="depth_method"      type="str"    value="$(arg depth_method)"/>
+    <param name="depth_device"      type="int"    value="$(arg depth_device)"/>
+    <param name="reg_method"        type="str"    value="$(arg reg_method)"/>
+    <param name="reg_device"        type="int"    value="$(arg reg_device)"/>
+    <param name="max_depth"         type="double" value="$(arg max_depth)"/>
+    <param name="min_depth"         type="double" value="$(arg min_depth)"/>
+    <param name="queue_size"        type="int"    value="$(arg queue_size)"/>
+    <param name="bilateral_filter"  type="bool"   value="$(arg bilateral_filter)"/>
+    <param name="edge_aware_filter" type="bool"   value="$(arg edge_aware_filter)"/>
+    <param name="worker_threads"    type="int"    value="$(arg worker_threads)"/>
+  </node>
+
+  <!-- sd point cloud (512 x 424) -->
+  <node pkg="nodelet" type="nodelet" name="$(arg base_name)_points_xyzrgb_sd" machine="$(arg machine)"
+        args="load depth_image_proc/point_cloud_xyzrgb $(arg nodelet_manager)" respawn="$(arg respawn)">
+    <remap from="rgb/camera_info"             to="$(arg base_name)/sd/camera_info"/>
+    <remap from="rgb/image_rect_color"        to="$(arg base_name)/sd/image_color_rect"/>
+    <remap from="depth_registered/image_rect" to="$(arg base_name)/sd/image_depth_rect"/>
+    <remap from="depth_registered/points"     to="$(arg base_name)/sd/points"/>
+    <param name="queue_size" type="int" value="$(arg queue_size)"/>
+  </node>
+
+  <!--************************ end ************************-->
+  <arg name="gui" default="true" />
+    <arg name="rvizconfig" default="$(find wpb_home_behaviors)/rviz/grab.rviz" />
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+</launch>

wpb_home_behaviors/src/wpb_home_objects_3d.cpp

@@ -153,7 +153,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
     int i = 0, nr_points = (int) cloud_source_ptr->points.size ();
     // While 30% of the original cloud is still there
-    while (cloud_source_ptr->points.size () > 0.03 * nr_points)
+    while (cloud_source_ptr->points.size () > 0.3 * nr_points)
     {
         // Segment the largest planar component from the remaining cloud
         segmentation.setInputCloud (cloud_source_ptr);
@@ -170,9 +170,12 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
         extract.setNegative (false);
         extract.filter (*plane);
         float plane_height = plane->points[0].z;
-        ROS_WARN("%d - plana: %d points. height =%.2f" ,i, plane->width * plane->height,plane_height);
+        ROS_INFO("%d - plana: %d points. height =%.2f" ,i, plane->width * plane->height,plane_height);
         if(plane_height > 0.6 && plane_height < 0.85) 
-        break;
+        {
+            ROS_WARN("Final plane: %d points. height =%.2f" , plane->width * plane->height,plane_height);
+            break;
+        }
 
         // Create the filtering object
         extract.setNegative (true);
@@ -180,6 +183,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
         cloud_source_ptr.swap (cloud_f);
         i++;
     }
+    
 
     if (planeIndices->indices.size() == 0)
         std::cout << "Could not find a plane in the scene." << std::endl;
@@ -219,7 +223,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
             pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
             std::vector<pcl::PointIndices> cluster_indices;
             ec.setClusterTolerance (0.05);
-            ec.setMinClusterSize (800);
+            ec.setMinClusterSize (500);
             ec.setMaxClusterSize (10000000);
             ec.setSearchMethod (tree);
             ec.setInputCloud (objects);
@@ -269,8 +273,8 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
                     std::string obj_id = stringStream.str();
                     float object_x = boxMarker.xMax;
                     float object_y = (boxMarker.yMin+boxMarker.yMax)/2;
-                    float object_z = boxMarker.zMin + 0.04;
+                    float object_z = boxMarker.zMin + 0.2;
-                    DrawText(obj_id,0.08, object_x,object_y,object_z, 1,0,1);
+                    DrawText(obj_id,0.06, object_x,object_y,object_z, 1,0,1);
                     tmpObj.name = obj_id;
                     tmpObj.x = object_x;
                     tmpObj.y = object_y;

wpb_home_tutorials/CMakeLists.txt

@@ -356,4 +356,20 @@ add_executable(wpb_home_behavior_node
 add_dependencies(wpb_home_behavior_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(wpb_home_behavior_node
   ${catkin_LIBRARIES}
-)
+)
+
+add_executable(wpb_home_obj_node
+  src/wpb_home_obj_node.cpp
+)
+add_dependencies(wpb_home_obj_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_obj_node
+  ${catkin_LIBRARIES}
+)
+
+add_executable(wpb_home_grab_node
+  src/wpb_home_grab_node.cpp
+)
+add_dependencies(wpb_home_grab_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_grab_node
+  ${catkin_LIBRARIES}
+)

wpb_home_tutorials/src/wpb_home_grab_node.cpp

@@ -0,0 +1,81 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the WaterPlus nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/* @author Zhang Wanjie                                             */
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Pose.h>
+#include <wpb_home_behaviors/Coord.h>
+
+static ros::Publisher grab_pub;
+static geometry_msgs::Pose grab_msg;
+static bool bGrabbing = false;
+
+void ObjCoordCB(const wpb_home_behaviors::Coord::ConstPtr &msg)
+{
+    if(bGrabbing == false)
+    {
+        int nNumObj = msg->name.size();
+        ROS_WARN("[ObjCoordCB] obj = %d",nNumObj);
+        if(nNumObj > 0)
+        {
+            ROS_WARN("[ObjCoordCB] Grab %s! (%.2f , %.2f , %.2f)",msg->name[0].c_str(),msg->x[0],msg->y[0],msg->z[0]);
+            grab_msg.position.x = msg->x[0];
+            grab_msg.position.y = msg->y[0];
+            grab_msg.position.z = msg->z[0];
+            grab_pub.publish(grab_msg);
+            bGrabbing = true;
+        }
+    }
+}
+
+void GrabResultCB(const std_msgs::String::ConstPtr &msg)
+{
+    //ROS_WARN("[GrabResultCB] %s",msg->data.c_str());
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "wpb_home_grab_node");
+    ROS_WARN("wpb_home_grab_node start!");
+
+    ros::NodeHandle n;
+    grab_pub = n.advertise<geometry_msgs::Pose>("/wpb_home/grab_action", 1);
+    ros::Subscriber obj_sub = n.subscribe("/wpb_home/objects_3d", 1, ObjCoordCB);
+    ros::Subscriber res_sub = n.subscribe("/wpb_home/grab_result", 30, GrabResultCB);
+
+    ros::spin();
+
+    return 0;
+}

wpb_home_tutorials/src/wpb_home_obj_node.cpp

@@ -0,0 +1,65 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the WaterPlus nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/* @author Zhang Wanjie                                             */
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <wpb_home_behaviors/Coord.h>
+
+void ObjCoordCB(const wpb_home_behaviors::Coord::ConstPtr &msg)
+{
+    int nNumObj = msg->name.size();
+    ROS_WARN("[ObjCoordCB] obj = %d",nNumObj);
+    if(nNumObj > 0)
+    {
+        for(int i=0 ; i<nNumObj ; i++)
+        {
+            ROS_WARN("[ObjCoordCB] %s - (%.2f , %.2f , %.2f)",msg->name[0].c_str(),msg->x[0],msg->y[0],msg->z[0]);
+        }
+        
+    }
+} 
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "wpb_home_obj_node");
+    ROS_WARN("wpb_home_obj_node start!");
+
+    ros::NodeHandle n;
+    ros::Subscriber obj_sub = n.subscribe("/wpb_home/objects_3d", 1, ObjCoordCB);
+
+    ros::spin();
+
+    return 0;
+}