ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

obj&grab demo

Browse Source
This commit is contained in:
ZhangWanjie 2019-07-20 18:58:26 +08:00
parent e6ebbdc384
commit f6ec90651d
5 changed files with 285 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
wpb_home_behaviors/launch/obj_3d.launch Normal file
View File

@ -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>

16
wpb_home_behaviors/src/wpb_home_objects_3d.cpp
View File

@ -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;
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;

18
wpb_home_tutorials/CMakeLists.txt
View File

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

81
wpb_home_tutorials/src/wpb_home_grab_node.cpp Normal file
View File

@ -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;
}

65
wpb_home_tutorials/src/wpb_home_obj_node.cpp Normal file
View File

@ -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;
}