new demo

wpb_home_tutorials/CMakeLists.txt

@@ -332,4 +332,28 @@ add_executable(wpb_home_grab_object
 add_dependencies(wpb_home_grab_object ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(wpb_home_grab_object
   ${catkin_LIBRARIES}
+)
+
+add_executable(wpb_home_image_node
+  src/wpb_home_image_node.cpp
+)
+add_dependencies(wpb_home_image_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_image_node
+  ${catkin_LIBRARIES}
+)
+
+add_executable(wpb_home_pointcloud_node
+  src/wpb_home_pointcloud_node.cpp
+)
+add_dependencies(wpb_home_pointcloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_pointcloud_node
+  ${catkin_LIBRARIES}
+)
+
+add_executable(wpb_home_behavior_node
+  src/wpb_home_behavior_node.cpp
+)
+add_dependencies(wpb_home_behavior_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_behavior_node
+  ${catkin_LIBRARIES}
 )

wpb_home_tutorials/src/wpb_home_behavior_node.cpp

@@ -0,0 +1,76 @@
+/*********************************************************************
+* 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 <std_msgs/String.h>
+#include <sensor_msgs/LaserScan.h>
+#include <geometry_msgs/Twist.h>
+
+ros::Publisher vel_pub;
+
+void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+{
+    int nNum = scan->ranges.size();
+    
+    int nMid = nNum/2;
+    float fMidDist = scan->ranges[nMid];
+    ROS_INFO("Point[%d] = %f", nMid, fMidDist); 
+
+    geometry_msgs::Twist vel_cmd;
+    if(fMidDist > 1.0f)
+    {
+        vel_cmd.linear.x = 0.05;
+    }
+    else
+    {
+        vel_cmd.angular.z = 0.1;
+    }
+    vel_pub.publish(vel_cmd);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc,argv,"wpb_home_behavior_node");
+    
+    ROS_INFO("wpb_home_behavior_node start!");
+
+    ros::NodeHandle nh;
+    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);
+    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);
+
+    ros::spin();
+}

wpb_home_tutorials/src/wpb_home_image_node.cpp

@@ -0,0 +1,70 @@
+/*********************************************************************
+* 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 <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/image_encodings.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+bool bCaptrueOneFrame = true;
+
+void callbackRGB(const sensor_msgs::ImageConstPtr& msg)
+{
+    if(bCaptrueOneFrame == true)
+    {
+        cv_bridge::CvImagePtr cv_ptr;
+        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+        imwrite("/home/robot/1.jpg",cv_ptr->image);
+        ROS_WARN("captrue image");
+        bCaptrueOneFrame = false;
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "wpb_home_image_node");
+    ROS_WARN("wpb_home_image_node start");
+
+    ros::NodeHandle nh;
+    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/hd/image_color", 1 , callbackRGB);
+
+    ros::spin();
+
+    return 0;
+
+}

wpb_home_tutorials/src/wpb_home_pointcloud_node.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/PointCloud2.h>
+#include <pcl_ros/point_cloud.h>
+
+void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg)
+{
+    pcl::PointCloud<pcl::PointXYZ> pointCloudIn;
+    pcl::fromROSMsg(*msg , pointCloudIn);
+
+    int cloudSize = pointCloudIn.points.size();
+    int pcIndex = cloudSize/2;
+    ROS_INFO("[pcIndex= %d] ( %.2f , %.2f , %.2f)", 
+        pcIndex , 
+        pointCloudIn.points[pcIndex].x, 
+        pointCloudIn.points[pcIndex].y, 
+        pointCloudIn.points[pcIndex].z);
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "wpb_home_pointcloud_node");
+    ROS_WARN("wpb_home_pointcloud_node start");
+
+    ros::NodeHandle nh;
+    ros::Subscriber pc_sub = nh.subscribe("/kinect2/sd/points", 1 , callbackPC);
+
+    ros::spin();
+
+    return 0;
+}