add drink_serve & joystick

wpb_home_behaviors/src/wpb_home_grab_server.cpp

@@ -160,7 +160,8 @@ static int nTimeOut = 0;
 
 void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
 {
-    //ROS_WARN("ProcCloudCB");
+    if(nStep == STEP_WAIT)
+        return;
 
     //to footprint
     sensor_msgs::PointCloud2 pc_footprint;
@@ -713,6 +714,12 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh_param("~");
     nh_param.param<std::string>("topic", pc_topic, "/kinect2/sd/points");
+    bool start_flag = false;
+    nh_param.param<bool>("start", start_flag, false);
+    if(start_flag == true)
+    {
+        nStep = STEP_FIND_PLANE;
+    }
 
     ros::NodeHandle nh;
     ros::Subscriber pc_sub = nh.subscribe(pc_topic, 10 , ProcCloudCB);

wpb_home_behaviors/src/wpb_home_objects_3d.cpp

@@ -36,6 +36,7 @@
  ********************************************************************/
 
 #include <ros/ros.h>
+#include <std_msgs/String.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 #include <boost/foreach.hpp>
@@ -68,6 +69,12 @@
 #include <math.h>
 #include <iostream>
 
+
+// 工作模式
+#define MODE_IDLE                   0
+#define MODE_OBJECT_DETECT 1
+static int nMode = MODE_IDLE;
+
 using namespace std;
 
 static std::string pc_topic;
@@ -114,7 +121,10 @@ static vector<stObjectDetected> arObj;
 
 void ProcCloudCB(const sensor_msgs::PointCloud2 &input)
 {
-    //ROS_WARN("ProcCloudCB");
+    //MODE_IDLE 不处理数据
+    if(nMode == MODE_IDLE)
+        return;
+
     //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)); 
@@ -451,6 +461,25 @@ void RemoveBoxes()
     marker_pub.publish(text_marker);
 }
 
+void BehaviorCB(const std_msgs::String::ConstPtr &msg)
+{
+    int nFindIndex = 0;
+    nFindIndex = msg->data.find("object_detect start");
+    if( nFindIndex >= 0 )
+    {
+        ROS_WARN("[object_detect start] ");
+        nMode = MODE_OBJECT_DETECT;
+    }
+
+    nFindIndex = msg->data.find("object_detect stop");
+    if( nFindIndex >= 0 )
+    {
+        ROS_WARN("[object_detect stop] ");
+        RemoveBoxes();
+        nMode = MODE_IDLE;
+    }
+}
+
 
 int main(int argc, char **argv)
 {
@@ -460,10 +489,16 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh_param("~");
     nh_param.param<std::string>("topic", pc_topic, "/kinect2/qhd/points");
+    bool start_flag = false;
+    nh_param.param<bool>("start", start_flag, false);
+    if(start_flag == true)
+    {
+        nMode = MODE_OBJECT_DETECT;
+    }
 
     ros::NodeHandle nh;
     ros::Subscriber pc_sub = nh.subscribe(pc_topic, 10 , ProcCloudCB);
-
+    ros::Subscriber beh_sub = nh.subscribe("/wpb_home/behaviors", 30, BehaviorCB);
     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);

wpb_home_tutorials/CMakeLists.txt

@@ -253,6 +253,14 @@ target_link_libraries(wpb_home_sr_xfyun
   ${catkin_LIBRARIES}
 )
 
+add_executable(wpb_home_joystick_demo
+  src/wpb_home_joystick_demo.cpp
+)
+add_dependencies(wpb_home_joystick_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_joystick_demo
+  ${catkin_LIBRARIES}
+)
+
 add_executable(wpb_home_velocity_control
   src/wpb_home_velocity_control.cpp
 )
@@ -277,7 +285,6 @@ target_link_libraries(wpb_home_speak
   ${catkin_LIBRARIES}
 )
 
-
 add_executable(wpb_home_imu_turn
   src/wpb_home_imu_turn.cpp
 )
@@ -405,3 +412,11 @@ add_dependencies(wpb_home_face_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin
 target_link_libraries(wpb_home_face_node
   ${catkin_LIBRARIES}
 )
+
+add_executable(wpb_home_drink_serve
+  src/wpb_home_drink_serve.cpp
+)
+add_dependencies(wpb_home_drink_serve ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_drink_serve
+  ${catkin_LIBRARIES}
+)

wpb_home_tutorials/src/wpb_home_drink_serve.cpp

@@ -0,0 +1,195 @@
+/*********************************************************************
+* 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 <std_msgs/Float64.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+#include <wpb_mani_behaviors/Coord.h>
+#include <sensor_msgs/JointState.h>
+
+static ros::Publisher behaviors_pub;
+static std_msgs::String behavior_msg;
+static ros::Publisher waypoint_pub;
+static ros::Publisher grab_drink_pub;
+static geometry_msgs::Pose grab_drink_msg;
+static ros::Publisher mani_ctrl_pub;
+static sensor_msgs::JointState mani_ctrl_msg;
+
+#define STEP_READY                                      0
+#define STEP_GOTO_KITCHEN                   1
+#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
+int nStep = STEP_READY;
+static int nDeley = 0;
+
+void DrinkCoordCB(const wpb_mani_behaviors::Coord::ConstPtr &msg)
+{
+    if(nStep == STEP_DRINK_DETECT)
+    {
+        // 获取饮料检测结果
+        int drink_num = msg->name.size();
+        ROS_INFO("[DrinkCoordCB] drink_num = %d",drink_num);
+        for(int i = 0;i<drink_num;i++)
+        {
+            ROS_INFO("[DrinkCoordCB]  %s  (%.2f , %.2f , %.2f)",msg->name[i].c_str(),msg->x[i],msg->y[i],msg->z[i]);
+        }
+        int grab_drink_index = 0;
+        grab_drink_msg.position.x = msg->x[grab_drink_index];
+        grab_drink_msg.position.y = msg->y[grab_drink_index];
+        grab_drink_msg.position.z = msg->z[grab_drink_index];
+        grab_drink_pub.publish(grab_drink_msg);
+
+        std_msgs::String behavior_msg;
+        behavior_msg.data = "object_detect stop";
+        behaviors_pub.publish(behavior_msg);
+        nStep = STEP_GRAB_DRINK;
+    }
+}
+
+void NaviResultCB(const std_msgs::String::ConstPtr &msg)
+{
+    if(nStep == STEP_GOTO_KITCHEN)
+    {
+        if(msg->data == "done")
+        {
+            ROS_INFO("[NaviResultCB] Waypoint kitchen!");
+            behavior_msg.data = "object_detect start";
+            behaviors_pub.publish(behavior_msg);
+            nStep = STEP_DRINK_DETECT;
+        }
+    }
+     
+    if(nStep == STEP_GOTO_DINNING_ROOM)
+    {
+        if(msg->data == "done")
+        {
+            ROS_INFO("[NaviResultCB] Waypoint dinning room!");
+            mani_ctrl_msg.name[0] = "gripper";
+            mani_ctrl_msg.position[0] = 0.15;   //张开手爪
+            mani_ctrl_pub.publish(mani_ctrl_msg);
+            nDeley = 0;
+            nStep = STEP_PUT_DOWN;
+        }
+    }
+}
+
+void GrabResultCB(const std_msgs::String::ConstPtr &msg)
+{
+    if(nStep == STEP_GRAB_DRINK)
+    {
+        if(msg->data == "done")
+        {
+            ROS_INFO("[GrabResultCB] grab_drink done!");
+            std_msgs::String waypoint_msg;
+            waypoint_msg.data = "dinning room";
+            waypoint_pub.publish(waypoint_msg);
+            nStep = STEP_GOTO_DINNING_ROOM;
+        }
+    }
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "wpb_home_drink_serve");
+
+    ros::NodeHandle n;
+    behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 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 drink_result_sub = n.subscribe("/wpb_home/objects_3d", 10 , DrinkCoordCB);
+    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);
+    mani_ctrl_pub = n.advertise<sensor_msgs::JointState>("/wpb_home/mani_ctrl", 10);
+    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
+
+    mani_ctrl_msg.name.resize(1);
+    mani_ctrl_msg.position.resize(1);
+    mani_ctrl_msg.velocity.resize(1);
+    mani_ctrl_msg.name[0] = "lift";
+    mani_ctrl_msg.position[0] = 0;
+    
+    sleep(1);
+
+    ros::Rate r(10);
+    while(ros::ok())
+    {
+        if(nStep == STEP_READY)
+        {
+            std_msgs::String waypoint_msg;
+            waypoint_msg.data = "kitchen";
+            waypoint_pub.publish(waypoint_msg);
+            nStep = STEP_GOTO_KITCHEN;
+        }
+
+        if(nStep == STEP_PUT_DOWN)
+        {
+            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;
+            }
+        }
+
+        if(nStep == STEP_BACKWARD)
+        {
+            geometry_msgs::Twist vel_cmd;
+            vel_cmd.linear.x = -0.1;
+            vel_pub.publish(vel_cmd);
+            nDeley ++;
+            if(nDeley > 5 *10)
+            {
+                mani_ctrl_msg.name[0] = "lift";
+                mani_ctrl_msg.position[0] = 0;            //收回手臂
+                mani_ctrl_pub.publish(mani_ctrl_msg);
+                nStep = STEP_DONE;
+            }
+        }
+
+        ros::spinOnce();
+        r.sleep();
+    }
+    
+    return 0;
+}

wpb_home_tutorials/src/wpb_home_grab_node.cpp

@@ -38,6 +38,7 @@
 #include <geometry_msgs/Pose.h>
 #include <wpb_home_behaviors/Coord.h>
 
+static ros::Publisher behaviors_pub;
 static ros::Publisher grab_pub;
 static geometry_msgs::Pose grab_msg;
 static bool bGrabbing = false;
@@ -56,6 +57,10 @@ void ObjCoordCB(const wpb_home_behaviors::Coord::ConstPtr &msg)
             grab_msg.position.z = msg->z[0];
             grab_pub.publish(grab_msg);
             bGrabbing = true;
+
+            std_msgs::String behavior_msg;
+            behavior_msg.data = "object_detect stop";
+            behaviors_pub.publish(behavior_msg);
         }
     }
 }
@@ -71,10 +76,17 @@ int main(int argc, char** argv)
     ROS_WARN("wpb_home_grab_node start!");
 
     ros::NodeHandle n;
+    behaviors_pub = n.advertise<std_msgs::String>("/wpb_home/behaviors", 10);
     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);
 
+    sleep(1);
+
+    std_msgs::String behavior_msg;
+    behavior_msg.data = "object_detect start";
+    behaviors_pub.publish(behavior_msg);
+
     ros::spin();
 
     return 0;

wpb_home_tutorials/src/wpb_home_joystick_demo.cpp

@@ -0,0 +1,67 @@
+/*********************************************************************
+* 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 <sensor_msgs/Joy.h>
+
+void JoyCallback(const sensor_msgs::Joy::ConstPtr& msg)
+{
+  printf("手柄摇杆： [0]%.2f  [1]%.2f  [2]%.2f  [3]%.2f\n",
+      msg->axes[0],
+      msg->axes[1],
+      msg->axes[2],
+      msg->axes[3]
+    );
+  printf("手柄按钮： [0]%d  [1]%d  [2]%d  [3]%d\n",
+      msg->buttons[0],
+      msg->buttons[1],
+      msg->buttons[2],
+      msg->buttons[3]
+    );
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "wpb_home_joystick_demo");
+
+  ros::NodeHandle n;
+  ros::Subscriber joy_sub = n.subscribe<sensor_msgs::Joy>("/joy", 10, JoyCallback);
+
+  ros::spin();
+
+  return 0;
+}