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

update serve_drinks

Browse Source
This commit is contained in:
zhangwanjie 2020-12-12 18:45:35 +08:00
parent e368b2146d
commit afc5a79605
2 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
wpb_home_tutorials/CMakeLists.txt
View File

@ -40,6 +40,7 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs geometry_msgs std_msgs visualization_msgs sensor_msgs geometry_msgs std_msgs visualization_msgs
image_geometry message_generation image_geometry message_generation
move_base_msgs actionlib move_base_msgs actionlib
wpb_home_behaviors
) )
## * add "message_runtime" and every package in MSG_DEP_SET to ## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...) ## catkin_package(CATKIN_DEPENDS ...)
@ -413,10 +414,10 @@ target_link_libraries(wpb_home_face_node
${catkin_LIBRARIES} ${catkin_LIBRARIES}
) )
add_executable(wpb_home_drink_serve add_executable(wpb_home_serve_drinks
src/wpb_home_drink_serve.cpp src/wpb_home_serve_drinks.cpp
) )
add_dependencies(wpb_home_drink_serve ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(wpb_home_serve_drinks ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_drink_serve target_link_libraries(wpb_home_serve_drinks
${catkin_LIBRARIES} ${catkin_LIBRARIES}
) )

15
wpb_home_tutorials/src/wpb_home_drink_serve.cpp → wpb_home_tutorials/src/wpb_home_serve_drinks.cpp
View File

@ -38,7 +38,7 @@
#include <std_msgs/Float64.h> #include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h> #include <geometry_msgs/Pose.h>
#include <wpb_mani_behaviors/Coord.h> #include <wpb_home_behaviors/Coord.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
static ros::Publisher behaviors_pub; static ros::Publisher behaviors_pub;
@ -54,13 +54,13 @@ static sensor_msgs::JointState mani_ctrl_msg;
#define STEP_DRINK_DETECT 2 #define STEP_DRINK_DETECT 2
#define STEP_GRAB_DRINK 3 #define STEP_GRAB_DRINK 3
#define STEP_GOTO_DINNING_ROOM 4 #define STEP_GOTO_DINNING_ROOM 4
#define STEP_PUT_DOWN 6 #define STEP_PUT_DOWN 5
#define STEP_BACKWARD 7 #define STEP_BACKWARD 6
#define STEP_DONE 8 #define STEP_DONE 7
int nStep = STEP_READY; int nStep = STEP_READY;
static int nDeley = 0; static int nDeley = 0;
void DrinkCoordCB(const wpb_mani_behaviors::Coord::ConstPtr &msg) void DrinkCoordCB(const wpb_home_behaviors::Coord::ConstPtr &msg)
{ {
if(nStep == STEP_DRINK_DETECT) if(nStep == STEP_DRINK_DETECT)
{ {
@ -132,9 +132,9 @@ int main(int argc, char** argv)
ros::NodeHandle n; ros::NodeHandle n;
behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 10); behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 10);
ros::Subscriber drink_result_sub = n.subscribe("/wpb_home/objects_3d", 10 , DrinkCoordCB);
waypoint_pub = n.advertise<std_msgs::String>("/waterplus/navi_waypoint", 10); waypoint_pub = n.advertise<std_msgs::String>("/waterplus/navi_waypoint", 10);
ros::Subscriber navi_res_sub = n.subscribe("/waterplus/navi_result", 10, NaviResultCB); ros::Subscriber navi_res_sub = n.subscribe("/waterplus/navi_result", 10, NaviResultCB);
ros::Subscriber drink_result_sub = n.subscribe("/wpb_home/objects_3d", 10 , DrinkCoordCB);
grab_drink_pub = n.advertise<geometry_msgs::Pose>("/wpb_home/grab_action", 1); grab_drink_pub = n.advertise<geometry_msgs::Pose>("/wpb_home/grab_action", 1);
ros::Subscriber grab_res_sub = n.subscribe("/wpb_home/grab_result", 10, GrabResultCB); ros::Subscriber grab_res_sub = n.subscribe("/wpb_home/grab_result", 10, GrabResultCB);
mani_ctrl_pub = n.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 10); mani_ctrl_pub = n.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 10);
@ -164,9 +164,6 @@ int main(int argc, char** argv)
nDeley ++; nDeley ++;
if(nDeley > 5*10) if(nDeley > 5*10)
{ {
mani_ctrl_msg.name[0] = "lift";
mani_ctrl_msg.position[0] = 0; //收回手臂
mani_ctrl_pub.publish(mani_ctrl_msg);
nDeley = 0; nDeley = 0;
nStep = STEP_BACKWARD; nStep = STEP_BACKWARD;
} }