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

new grab demo

Browse Source
This commit is contained in:
ZhangWanjie 2019-06-28 11:04:13 +08:00
parent 3081eef7f6
commit 4cc6d5952e
9 changed files with 1078 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
wpb_home_behaviors/CMakeLists.txt
View File

@ -53,11 +53,10 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
add_message_files(
FILES
Coord.msg
)
## Generate services in the 'srv' folder
# add_service_files(
@ -111,7 +110,7 @@ generate_messages(
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES wpb_home_behaviors
CATKIN_DEPENDS message_runtime
CATKIN_DEPENDS roscpp std_msgs message_runtime
# DEPENDS system_lib
)
@ -221,4 +220,21 @@ add_executable(wpb_home_pass_server
add_dependencies(wpb_home_pass_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_pass_server
${catkin_LIBRARIES}
)
add_executable(wpb_home_objects_3d
src/wpb_home_objects_3d.cpp
)
add_dependencies(wpb_home_objects_3d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_objects_3d
${catkin_LIBRARIES}
)
add_executable(wpb_home_grab_action
src/wpb_home_grab_action.cpp
)
add_dependencies(wpb_home_grab_action ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_grab_action
${catkin_LIBRARIES}
)

114
wpb_home_behaviors/launch/grab_action.launch Normal file
View File

@ -0,0 +1,114 @@
<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">
<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>
<node pkg="wpb_home_tutorials" type="wpb_home_grab_object" name="wpb_home_grab_object" output = "screen"/>
<!--- 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>

5
wpb_home_behaviors/msg/Coord.msg Normal file
View File

@ -0,0 +1,5 @@
string[] name
float64[] x
float64[] y
float64[] z
float64[] probability

75
wpb_home_behaviors/rviz/grab.rviz
View File

@ -12,7 +12,7 @@ Panels:
- /Plane1
- /Obj_Marker1
Splitter Ratio: 0.5
Tree Height: 767
Tree Height: 753
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@ -21,7 +21,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
@ -31,7 +31,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Kinect
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
@ -41,7 +41,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
@ -73,7 +73,7 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
forearm_link:
body_link:
Alpha: 1
Show Axes: false
Show Trail: false
@ -98,31 +98,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
mani_left_finger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
mani_right_finger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
upperarm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
@ -146,7 +121,7 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 12.6047001
Max Value: 12.6047
Min Value: 1.18583
Value: true
Axis: X
@ -166,7 +141,7 @@ Visualization Manager:
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Size (m): 0.01
Style: Flat Squares
Topic: /kinect2/sd/points
Unreliable: false
@ -176,8 +151,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.953278005
Min Value: -6.74629021
Max Value: 0.953278
Min Value: -6.74629
Value: true
Axis: Z
Channel Name: intensity
@ -185,7 +160,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
@ -196,13 +171,13 @@ Visualization Manager:
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Size (m): 0.01
Style: Flat Squares
Topic: /segmented_plane
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /obj_marker
@ -235,24 +210,22 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 3.5450201
Distance: 3.54502
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.30475998
Y: 0.345981985
Z: 0.364789009
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
X: 1.30476
Y: 0.345982
Z: 0.364789
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.519797623
Near Clip Distance: 0.01
Pitch: 0.519798
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.62544489
Yaw: 2.62544
Saved: ~
Window Geometry:
Displays:
@ -260,7 +233,7 @@ Window Geometry:
Height: 1056
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001920000038efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000038e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000390fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000390000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000046fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005a70000038e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000019200000389fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000389000000e600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000390fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000390000000be00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000072f00000046fc0100000002fb0000000800540069006d006501000000000000072f0000038800fffffffb0000000800540069006d00650100000000000004500000000000000000000005970000038900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -269,6 +242,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
Width: 1855
X: 65
Y: 24
Width: 1839
X: 68
Y: 17

350
wpb_home_behaviors/src/wpb_home_grab_action.cpp Normal file
View File

@ -0,0 +1,350 @@
/*********************************************************************
* 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 ZhangWanjie
********************************************************************/
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>
// 抓取参数调节（单位：米）(Modified in wpb_home.yaml!!)
static float grab_y_offset = 0.0f; //抓取前，对准物品，机器人的横向位移偏移量
static float grab_lift_offset = 0.0f; //手臂抬起高度的补偿偏移量
static float grab_forward_offset = 0.0f; //手臂抬起后，机器人向前抓取物品移动的位移偏移量
static float grab_gripper_value = 0.032; //抓取物品时，手爪闭合后的手指间距
#define STEP_WAIT 0
#define STEP_FIND_PLANE 1
#define STEP_PLANE_DIST 2
#define STEP_FIND_OBJ 3
#define STEP_OBJ_DIST 4
#define STEP_HAND_UP 5
#define STEP_FORWARD 6
#define STEP_GRAB 7
#define STEP_OBJ_UP 8
#define STEP_BACKWARD 9
#define STEP_DONE 10
static int nStep = STEP_WAIT;
static std::string pc_topic;
static ros::Publisher vel_pub;
static ros::Publisher mani_ctrl_pub;
static sensor_msgs::JointState mani_ctrl_msg;
static ros::Publisher result_pub;
void VelCmd(float inVx , float inVy, float inTz);
static std_msgs::String result_msg;
static ros::Publisher odom_ctrl_pub;
static std_msgs::String ctrl_msg;
static geometry_msgs::Pose2D pose_diff;
static float fObjGrabX = 0;
static float fObjGrabY = 0;
static float fObjGrabZ = 0;
static float fMoveTargetX = 0;
static float fMoveTargetY = 0;
static int nTimeDelayCounter = 0;
static float fPlaneDist = 0;
static float fTargetPlaneDist = 0.6; //与桌子之间的目标距离
static float fTargetGrabX = 0.9; //抓取时目标物品的x坐标
static float fTargetGrabY = 0.0; //抓取时目标物品的y坐标
void GrabActionCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
// 目标物品的坐标
fObjGrabX = msg->position.x;
fObjGrabY = msg->position.y;
fObjGrabZ = msg->position.z;
ROS_WARN("[OBJ_TO_GRAB] x = %.2f y= %.2f ,z= %.2f " ,fObjGrabX, fObjGrabY, fObjGrabZ);
ctrl_msg.data = "pose_diff reset";
odom_ctrl_pub.publish(ctrl_msg);
// ajudge the dist to obj
fMoveTargetX = fObjGrabX - fTargetGrabX;
fMoveTargetY = fObjGrabY - fTargetGrabY + grab_y_offset;
ROS_WARN("[MOVE_TARGET] x = %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
nStep = STEP_OBJ_DIST;
}
void PoseDiffCallback(const geometry_msgs::Pose2D::ConstPtr& msg)
{
pose_diff.x = msg->x;
pose_diff.y = msg->y;
pose_diff.theta = msg->theta;
}
void VelCmd(float inVx , float inVy, float inTz)
{
geometry_msgs::Twist vel_cmd;
vel_cmd.linear.x = inVx;
vel_cmd.linear.y = inVy;
vel_cmd.angular.z = inTz;
vel_pub.publish(vel_cmd);
}
void BehaviorCB(const std_msgs::String::ConstPtr &msg)
{
int nFindIndex = msg->data.find("grab stop");
if( nFindIndex >= 0 )
{
ROS_WARN("[grab_stop] ");
nStep = STEP_WAIT;
geometry_msgs::Twist vel_cmd;
vel_cmd.linear.x = 0;
vel_cmd.linear.y = 0;
vel_cmd.linear.z = 0;
vel_cmd.angular.x = 0;
vel_cmd.angular.y = 0;
vel_cmd.angular.z = 0;
vel_pub.publish(vel_cmd);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "wpb_home_grab_action");
ROS_INFO("wpb_home_grab_action");
ros::NodeHandle nh;
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 30);
mani_ctrl_pub = nh.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 30);
result_pub = nh.advertise<std_msgs::String>("/wpb_home/grab_result", 30);
ros::Subscriber sub_grab_pose = nh.subscribe("/wpb_home/grab_action", 1, GrabActionCallback);
ros::Subscriber sub_beh = nh.subscribe("/wpb_home/behaviors", 30, BehaviorCB);
odom_ctrl_pub = nh.advertise<std_msgs::String>("/wpb_home/ctrl", 30);
ros::Subscriber pose_diff_sub = nh.subscribe("/wpb_home/pose_diff", 1, PoseDiffCallback);
mani_ctrl_msg.name.resize(2);
mani_ctrl_msg.position.resize(2);
mani_ctrl_msg.velocity.resize(2);
mani_ctrl_msg.name[0] = "lift";
mani_ctrl_msg.name[1] = "gripper";
mani_ctrl_msg.position[0] = 0;
mani_ctrl_msg.velocity[0] = 0.5; //升降速度(单位:米/秒)
mani_ctrl_msg.position[1] = 0.16;
mani_ctrl_msg.velocity[1] = 5; //手爪开合角速度(单位:度/秒)
ros::NodeHandle nh_param("~");
nh_param.getParam("grab/grab_y_offset", grab_y_offset);
nh_param.getParam("grab/grab_lift_offset", grab_lift_offset);
nh_param.getParam("grab/grab_forward_offset", grab_forward_offset);
nh_param.getParam("grab/grab_gripper_value", grab_gripper_value);
ros::Rate r(30);
while(nh.ok())
{
//4、左右平移对准目标物品
if(nStep == STEP_OBJ_DIST)
{
float vx,vy;
vx = (fMoveTargetX - pose_diff.x)/2;
vy = (fMoveTargetY - pose_diff.y)/2;
VelCmd(vx,vy,0);
//ROS_INFO("[MOVE] T(%.2f %.2f) od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
{
VelCmd(0,0,0);
ctrl_msg.data = "pose_diff reset";
odom_ctrl_pub.publish(ctrl_msg);
nTimeDelayCounter = 0;
nStep = STEP_HAND_UP;
}
result_msg.data = "object x";
result_pub.publish(result_msg);
}
//5、抬起手臂
if(nStep == STEP_HAND_UP)
{
if(nTimeDelayCounter == 0)
{
mani_ctrl_msg.position[0] = fObjGrabZ + grab_lift_offset;
mani_ctrl_msg.position[1] = 0.16;
mani_ctrl_pub.publish(mani_ctrl_msg);
ROS_WARN("[STEP_HAND_UP] lift= %.2f gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);
result_msg.data = "hand up";
result_pub.publish(result_msg);
}
nTimeDelayCounter ++;
mani_ctrl_pub.publish(mani_ctrl_msg);
VelCmd(0,0,0);
if(nTimeDelayCounter > 15*30)
{
fMoveTargetX = fTargetGrabX -0.65 + grab_forward_offset;
fMoveTargetY = 0;
ROS_WARN("[STEP_FORWARD] x = %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
nTimeDelayCounter = 0;
//ctrl_msg.data = "pose_diff reset";
//odom_ctrl_pub.publish(ctrl_msg);
nStep = STEP_FORWARD;
}
}
//6、前进靠近物品
if(nStep == STEP_FORWARD)
{
float vx,vy;
vx = (fMoveTargetX - pose_diff.x)/2;
vy = (fMoveTargetY - pose_diff.y)/2;
VelCmd(vx,vy,0);
//ROS_INFO("[STEP_FORWARD] T(%.2f %.2f) od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
{
VelCmd(0,0,0);
ctrl_msg.data = "pose_diff reset";
odom_ctrl_pub.publish(ctrl_msg);
nTimeDelayCounter = 0;
ROS_WARN("[STEP_GRAB] grab_gripper_value = %.2f",grab_gripper_value);
nStep = STEP_GRAB;
}
result_msg.data = "forward";
result_pub.publish(result_msg);
}
//7、抓取物品
if(nStep == STEP_GRAB)
{
if(nTimeDelayCounter == 0)
{
result_msg.data = "grab";
result_pub.publish(result_msg);
}
mani_ctrl_msg.position[1] = grab_gripper_value; //抓取物品手爪闭合宽度
mani_ctrl_pub.publish(mani_ctrl_msg);
//ROS_WARN("[STEP_GRAB] lift= %.2f gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);
nTimeDelayCounter++;
VelCmd(0,0,0);
if(nTimeDelayCounter > 8*30)
{
nTimeDelayCounter = 0;
ROS_WARN("[STEP_OBJ_UP]");
nStep = STEP_OBJ_UP;
}
}
//8、拿起物品
if(nStep == STEP_OBJ_UP)
{
if(nTimeDelayCounter == 0)
{
mani_ctrl_msg.position[0] += 0.03;
mani_ctrl_pub.publish(mani_ctrl_msg);
//ROS_WARN("[MANI_CTRL] lift= %.2f gripper= %.2f " ,mani_ctrl_msg.position[0], mani_ctrl_msg.position[1]);
result_msg.data = "object up";
result_pub.publish(result_msg);
}
nTimeDelayCounter++;
VelCmd(0,0,0);
mani_ctrl_pub.publish(mani_ctrl_msg);
if(nTimeDelayCounter > 3*30)
{
fMoveTargetX = -(fObjGrabX -0.63 + grab_forward_offset);
fMoveTargetY = -(fObjGrabY + grab_y_offset);
ROS_WARN("[STEP_BACKWARD] x= %.2f y= %.2f " ,fMoveTargetX, fMoveTargetY);
nTimeDelayCounter = 0;
nStep = STEP_BACKWARD;
}
}
//9、带着物品后退
if(nStep == STEP_BACKWARD)
{
//ROS_WARN("[STEP_BACKWARD] nTimeDelayCounter = %d " ,nTimeDelayCounter);
//nTimeDelayCounter++;
float vx,vy;
vx = (fMoveTargetX - pose_diff.x)/2;
vy = (fMoveTargetY - pose_diff.y)/2;
VelCmd(vx,vy,0);
//ROS_INFO("[MOVE] T(%.2f %.2f) od(%.2f , %.2f) v(%.2f,%.2f)" ,fMoveTargetX, fMoveTargetY, pose_diff.x ,pose_diff.y,vx,vy);
if(fabs(vx) < 0.01 && fabs(vy) < 0.01)
{
VelCmd(0,0,0);
ctrl_msg.data = "pose_diff reset";
odom_ctrl_pub.publish(ctrl_msg);
nTimeDelayCounter = 0;
ROS_WARN("[STEP_DONE]");
nStep = STEP_DONE;
}
result_msg.data = "backward";
result_pub.publish(result_msg);
}
//10、抓取任务完毕
if(nStep == STEP_DONE)
{
if(nTimeDelayCounter < 30)
{
VelCmd(0,0,0);
nTimeDelayCounter ++;
}
result_msg.data = "done";
result_pub.publish(result_msg);
}
ros::spinOnce();
r.sleep();
}
return 0;
}

460
wpb_home_behaviors/src/wpb_home_objects_3d.cpp Normal file
View File

@ -0,0 +1,460 @@
/*********************************************************************
* 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 ZhangWanjie
********************************************************************/
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <image_geometry/pinhole_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <sensor_msgs/Image.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <wpb_home_behaviors/Coord.h>
#include "highgui.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
using namespace std;
static std::string pc_topic;
static ros::Publisher pc_pub;
static ros::Publisher marker_pub;
static ros::Publisher coord_pub;
static tf::TransformListener *tf_listener;
void DrawBox(float inMinX, float inMaxX, float inMinY, float inMaxY, float inMinZ, float inMaxZ, float inR, float inG, float inB);
void DrawText(std::string inText, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB);
void DrawPath(float inX, float inY, float inZ);
void RemoveBoxes();
void SortObjects();
static visualization_msgs::Marker line_box;
static visualization_msgs::Marker line_follow;
static visualization_msgs::Marker text_marker;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
static ros::Publisher segmented_objects;
static ros::Publisher segmented_plane;
typedef struct stBoxMarker
{
float xMax;
float xMin;
float yMax;
float yMin;
float zMax;
float zMin;
}stBoxMarker;
static stBoxMarker boxMarker;
typedef struct stObjectDetected
{
string name;
float x;
float y;
float z;
float probability;
}stObjectDetected;
static stObjectDetected tmpObj;
static vector<stObjectDetected> arObj;
void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
{
//ROS_WARN("ProcCloudCB");
//to footprint
sensor_msgs::PointCloud2 pc_footprint;
bool res = tf_listener->waitForTransform("/base_footprint", input.header.frame_id, input.header.stamp, ros::Duration(5.0));
if(res == false)
{
return;
}
pcl_ros::transformPointCloud("/base_footprint", input, pc_footprint, *tf_listener);
//source cloud
pcl::PointCloud<pcl::PointXYZRGB> cloud_src;
pcl::fromROSMsg(pc_footprint , cloud_src);
//ROS_INFO("cloud_src size = %d",cloud_src.size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source_ptr;
cloud_source_ptr = cloud_src.makeShared();
//process
//printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr convexHull(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr objects(new pcl::PointCloud<pcl::PointXYZRGB>);
// Get the plane model, if present.
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZRGB> segmentation;
segmentation.setInputCloud(cloud_source_ptr);
segmentation.setModelType(pcl::SACMODEL_PLANE);
segmentation.setMethodType(pcl::SAC_RANSAC);
segmentation.setDistanceThreshold(0.005);
segmentation.setOptimizeCoefficients(true);
pcl::PointIndices::Ptr planeIndices(new pcl::PointIndices);
segmentation.segment(*planeIndices, *coefficients);
ROS_INFO_STREAM("Planes: " << planeIndices->indices.size());
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
////////////////////////////////////////////////////////////////////////////
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)
{
// Segment the largest planar component from the remaining cloud
segmentation.setInputCloud (cloud_source_ptr);
segmentation.segment (*planeIndices, *coefficients);
if (planeIndices->indices.size () == 0)
{
ROS_WARN("Could not estimate a planar model for the given dataset.");
break;
}
// Extract the planeIndices
extract.setInputCloud (cloud_source_ptr);
extract.setIndices (planeIndices);
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);
if(plane_height > 0.6 && plane_height < 0.85)
break;
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_source_ptr.swap (cloud_f);
i++;
}
if (planeIndices->indices.size() == 0)
std::cout << "Could not find a plane in the scene." << std::endl;
else
{
// Copy the points of the plane to a new cloud.
extract.setInputCloud(cloud_source_ptr);
extract.setIndices(planeIndices);
extract.filter(*plane);
// Retrieve the convex hull.
pcl::ConvexHull<pcl::PointXYZRGB> hull;
hull.setInputCloud(plane);
// Make sure that the resulting hull is bidimensional.
hull.setDimension(2);
hull.reconstruct(*convexHull);
// Redundant check.
if (hull.getDimension() == 2)
{
// Prism object.
pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism;
prism.setInputCloud(cloud_source_ptr);
prism.setInputPlanarHull(convexHull);
prism.setHeightLimits(-0.30, -0.05); //height limit objects lying on the plane
pcl::PointIndices::Ptr objectIndices(new pcl::PointIndices);
// Get and show all points retrieved by the hull.
prism.segment(*objectIndices);
extract.setIndices(objectIndices);
extract.filter(*objects);
segmented_objects.publish(objects);
segmented_plane.publish(plane);
// run clustering extraction on "objects" to get several pointclouds
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
std::vector<pcl::PointIndices> cluster_indices;
ec.setClusterTolerance (0.05);
ec.setMinClusterSize (800);
ec.setMaxClusterSize (10000000);
ec.setSearchMethod (tree);
ec.setInputCloud (objects);
ec.extract (cluster_indices);
pcl::ExtractIndices<pcl::PointXYZRGB> extract_object_indices;
std::vector<pcl::PointCloud<pcl::PointXYZRGB> > objectf;
cv::Mat element;
RemoveBoxes();
arObj.clear();
int nObjCnt = 0;
for(int i = 0; i<cluster_indices.size(); ++i)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
extract_object_indices.setInputCloud(objects);
extract_object_indices.setIndices(boost::make_shared<const pcl::PointIndices>(cluster_indices[i]));
extract_object_indices.filter(*object_cloud);
objectf.push_back(*object_cloud);
bool bFirstPoint = true;
for (int j = 0; j < object_cloud->points.size(); j++)
{
pcl::PointXYZRGB p = object_cloud->points[j];
if(bFirstPoint == true)
{
boxMarker.xMax = boxMarker.xMin = p.x;
boxMarker.yMax = boxMarker.yMin = p.y;
boxMarker.zMax = boxMarker.zMin = p.z;
bFirstPoint = false;
}
if(p.x < boxMarker.xMin) { boxMarker.xMin = p.x;}
if(p.x > boxMarker.xMax) { boxMarker.xMax = p.x;}
if(p.y < boxMarker.yMin) { boxMarker.yMin = p.y;}
if(p.y > boxMarker.yMax) { boxMarker.yMax = p.y;}
if(p.z < boxMarker.zMin) { boxMarker.zMin = p.z;}
if(p.z > boxMarker.zMax) { boxMarker.zMax = p.z;}
}
if(boxMarker.xMin < 1.5 && boxMarker.yMin > -0.5 && boxMarker.yMax < 0.5)
{
DrawBox(boxMarker.xMin, boxMarker.xMax, boxMarker.yMin, boxMarker.yMax, boxMarker.zMin, boxMarker.zMax, 0, 1, 0);
std::ostringstream stringStream;
stringStream << "obj_" << nObjCnt;
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);
tmpObj.name = obj_id;
tmpObj.x = object_x;
tmpObj.y = object_y;
tmpObj.z = object_z;
tmpObj.probability = 1.0f;
arObj.push_back(tmpObj);
// coord.name.push_back(obj_id);
// coord.x.push_back(object_x);
// coord.y.push_back(object_y);
// coord.z.push_back(object_z);
// coord.probability.push_back(1.0f);
nObjCnt++;
ROS_WARN("[obj_%d] xMin= %.2f yMin = %.2f yMax = %.2f",i,boxMarker.xMin, boxMarker.yMin, boxMarker.yMax);
}
}
SortObjects();
// coord_pub.publish(coord);
}
else std::cout << "The chosen hull is not planar." << std::endl;
}
}
float CalObjDist(stObjectDetected* inObj)
{
float x = inObj->x;
float y = inObj->y;
float z = inObj->z - 0.8f;
float dist = sqrt(x*x + y*y + z*z);
return dist;
}
void SortObjects()
{
int nNum = arObj.size();
if (nNum == 0)
return;
// 冒泡排序
stObjectDetected tObj;
for(int n = 0; n<nNum; n++)
{
float minObjDist = CalObjDist(&arObj[n]);
for(int i=n+1;i<nNum; i++)
{
float curDist = CalObjDist(&arObj[i]);
if(curDist < minObjDist)
{
// 交换位置
tObj = arObj[n];
arObj[n] = arObj[i];
arObj[i] = tObj;
minObjDist = curDist;
}
}
}
// 排序完毕，发送消息
wpb_home_behaviors::Coord coord;
for(int i=0;i<nNum; i++)
{
coord.name.push_back(arObj[i].name);
coord.x.push_back(arObj[i].x);
coord.y.push_back(arObj[i].y);
coord.z.push_back(arObj[i].z);
coord.probability.push_back(arObj[i].probability);
}
coord_pub.publish(coord);
}
void DrawBox(float inMinX, float inMaxX, float inMinY, float inMaxY, float inMinZ, float inMaxZ, float inR, float inG, float inB)
{
line_box.header.frame_id = "base_footprint";
line_box.ns = "line_box";
line_box.action = visualization_msgs::Marker::ADD;
line_box.id = 0;
line_box.type = visualization_msgs::Marker::LINE_LIST;
line_box.scale.x = 0.005;
line_box.color.r = inR;
line_box.color.g = inG;
line_box.color.b = inB;
line_box.color.a = 1.0;
geometry_msgs::Point p;
p.z = inMinZ;
p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
p.z = inMaxZ;
p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMinY; line_box.points.push_back(p);
p.x = inMinX; p.y = inMinY; p.z = inMinZ; line_box.points.push_back(p);
p.x = inMinX; p.y = inMinY; p.z = inMaxZ; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; p.z = inMinZ; line_box.points.push_back(p);
p.x = inMinX; p.y = inMaxY; p.z = inMaxZ; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; p.z = inMinZ; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMaxY; p.z = inMaxZ; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; p.z = inMinZ; line_box.points.push_back(p);
p.x = inMaxX; p.y = inMinY; p.z = inMaxZ; line_box.points.push_back(p);
marker_pub.publish(line_box);
}
static int nTextNum = 2;
void DrawText(std::string inText, float inScale, float inX, float inY, float inZ, float inR, float inG, float inB)
{
text_marker.header.frame_id = "base_footprint";
text_marker.ns = "line_obj";
text_marker.action = visualization_msgs::Marker::ADD;
text_marker.id = nTextNum;
nTextNum ++;
text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_marker.scale.z = inScale;
text_marker.color.r = inR;
text_marker.color.g = inG;
text_marker.color.b = inB;
text_marker.color.a = 1.0;
text_marker.pose.position.x = inX;
text_marker.pose.position.y = inY;
text_marker.pose.position.z = inZ;
text_marker.pose.orientation=tf::createQuaternionMsgFromYaw(1.0);
text_marker.text = inText;
marker_pub.publish(text_marker);
}
void RemoveBoxes()
{
line_box.action = 3;
line_box.points.clear();
marker_pub.publish(line_box);
line_follow.action = 3;
line_follow.points.clear();
marker_pub.publish(line_follow);
text_marker.action = 3;
marker_pub.publish(text_marker);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "wpb_home_objects_3d");
ROS_INFO("wpb_home_objects_3d start!");
tf_listener = new tf::TransformListener();
ros::NodeHandle nh_param("~");
nh_param.param<std::string>("topic", pc_topic, "/kinect2/sd/points");
ros::NodeHandle nh;
ros::Subscriber pc_sub = nh.subscribe(pc_topic, 10 , ProcCloudCB);
pc_pub = nh.advertise<sensor_msgs::PointCloud2>("obj_pointcloud",1);
marker_pub = nh.advertise<visualization_msgs::Marker>("obj_marker", 10);
coord_pub = nh.advertise<wpb_home_behaviors::Coord>("/wpb_home/objects_3d", 10);
segmented_objects = nh.advertise<PointCloud> ("segmented_objects",1);
segmented_plane = nh.advertise<PointCloud> ("segmented_plane",1);
ros::spin();
delete tf_listener;
return 0;
}

4
wpb_home_bringup/config/wpb_home.yaml
View File

@ -1,6 +1,6 @@
zeros:
kinect_height: 1.38
kinect_pitch: -0.50
kinect_height: 1.37
kinect_pitch: -0.70
grab:
grab_y_offset: 0.0 #抓取前，对准物品，机器人的横向位移偏移量。大-往左修正；小-往右修正

8
wpb_home_tutorials/CMakeLists.txt
View File

@ -325,3 +325,11 @@ add_dependencies(wpb_home_lidar_behavior ${${PROJECT_NAME}_EXPORTED_TARGETS} ${c
target_link_libraries(wpb_home_lidar_behavior
${catkin_LIBRARIES}
)
add_executable(wpb_home_grab_object
src/wpb_home_grab_object.cpp
)
add_dependencies(wpb_home_grab_object ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_grab_object
${catkin_LIBRARIES}
)

93
wpb_home_tutorials/src/wpb_home_grab_object.cpp Normal file
View File

@ -0,0 +1,93 @@
/*********************************************************************
* 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 ZhangWanjie
********************************************************************/
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/String.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_object"); //程序初始化
ROS_WARN("wpb_home_grab_object 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);
// grab_msg.position.x = 0.9;
// grab_msg.position.y = -0.1;
// grab_msg.position.z = 0.8;
// grab_pub.publish(grab_msg);
ros::Rate r(30);
while(ros::ok())
{
ros::spinOnce();
r.sleep();
}
return 0;
}