diff --git a/wpb_home_behaviors/launch/obj_3d.launch b/wpb_home_behaviors/launch/obj_3d.launch
new file mode 100644
index 0000000..ed9d3f0
--- /dev/null
+++ b/wpb_home_behaviors/launch/obj_3d.launch
@@ -0,0 +1,112 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/wpb_home_behaviors/src/wpb_home_objects_3d.cpp b/wpb_home_behaviors/src/wpb_home_objects_3d.cpp
index e5c2d4e..9b67339 100644
--- a/wpb_home_behaviors/src/wpb_home_objects_3d.cpp
+++ b/wpb_home_behaviors/src/wpb_home_objects_3d.cpp
@@ -153,7 +153,7 @@ void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
pcl::PointCloud::Ptr cloud_f (new pcl::PointCloud);
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 ec;
std::vector 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;
- DrawText(obj_id,0.08, object_x,object_y,object_z, 1,0,1);
+ float object_z = boxMarker.zMin + 0.2;
+ 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;
diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt
index c6db740..41a6686 100644
--- a/wpb_home_tutorials/CMakeLists.txt
+++ b/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}
-)
\ No newline at end of file
+)
+
+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}
+)
diff --git a/wpb_home_tutorials/src/wpb_home_grab_node.cpp b/wpb_home_tutorials/src/wpb_home_grab_node.cpp
new file mode 100644
index 0000000..cd90739
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+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("/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;
+}
diff --git a/wpb_home_tutorials/src/wpb_home_obj_node.cpp b/wpb_home_tutorials/src/wpb_home_obj_node.cpp
new file mode 100644
index 0000000..eeb509b
--- /dev/null
+++ b/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
+#include
+#include
+
+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 ; iname[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;
+}