diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt index 15380b8..ec254c6 100644 --- a/wpb_home_tutorials/CMakeLists.txt +++ b/wpb_home_tutorials/CMakeLists.txt @@ -40,6 +40,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs geometry_msgs std_msgs visualization_msgs image_geometry message_generation move_base_msgs actionlib + wpb_home_behaviors ) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) @@ -413,10 +414,10 @@ target_link_libraries(wpb_home_face_node ${catkin_LIBRARIES} ) -add_executable(wpb_home_drink_serve - src/wpb_home_drink_serve.cpp +add_executable(wpb_home_serve_drinks + src/wpb_home_serve_drinks.cpp ) -add_dependencies(wpb_home_drink_serve ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(wpb_home_drink_serve +add_dependencies(wpb_home_serve_drinks ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(wpb_home_serve_drinks ${catkin_LIBRARIES} ) diff --git a/wpb_home_tutorials/src/wpb_home_drink_serve.cpp b/wpb_home_tutorials/src/wpb_home_serve_drinks.cpp similarity index 93% rename from wpb_home_tutorials/src/wpb_home_drink_serve.cpp rename to wpb_home_tutorials/src/wpb_home_serve_drinks.cpp index f96f311..7d11ab6 100644 --- a/wpb_home_tutorials/src/wpb_home_drink_serve.cpp +++ b/wpb_home_tutorials/src/wpb_home_serve_drinks.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include static ros::Publisher behaviors_pub; @@ -54,13 +54,13 @@ static sensor_msgs::JointState mani_ctrl_msg; #define STEP_DRINK_DETECT 2 #define STEP_GRAB_DRINK 3 #define STEP_GOTO_DINNING_ROOM 4 -#define STEP_PUT_DOWN 6 -#define STEP_BACKWARD 7 -#define STEP_DONE 8 +#define STEP_PUT_DOWN 5 +#define STEP_BACKWARD 6 +#define STEP_DONE 7 int nStep = STEP_READY; 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) { @@ -132,9 +132,9 @@ int main(int argc, char** argv) ros::NodeHandle n; behaviors_pub = n.advertise("/wpb_home/behaviors", 10); + ros::Subscriber drink_result_sub = n.subscribe("/wpb_home/objects_3d", 10 , DrinkCoordCB); waypoint_pub = n.advertise("/waterplus/navi_waypoint", 10); 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("/wpb_home/grab_action", 1); ros::Subscriber grab_res_sub = n.subscribe("/wpb_home/grab_result", 10, GrabResultCB); mani_ctrl_pub = n.advertise("/wpb_home/mani_ctrl", 10); @@ -164,9 +164,6 @@ int main(int argc, char** argv) nDeley ++; 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; nStep = STEP_BACKWARD; }