From e6ebbdc38489d9fc2d051beac1454da2547b1174 Mon Sep 17 00:00:00 2001 From: ZhangWanjie Date: Thu, 11 Jul 2019 17:32:25 +0800 Subject: [PATCH] new demo --- wpb_home_tutorials/CMakeLists.txt | 24 ++++++ .../src/wpb_home_behavior_node.cpp | 76 +++++++++++++++++++ .../src/wpb_home_image_node.cpp | 70 +++++++++++++++++ .../src/wpb_home_pointcloud_node.cpp | 67 ++++++++++++++++ 4 files changed, 237 insertions(+) create mode 100644 wpb_home_tutorials/src/wpb_home_behavior_node.cpp create mode 100644 wpb_home_tutorials/src/wpb_home_image_node.cpp create mode 100644 wpb_home_tutorials/src/wpb_home_pointcloud_node.cpp diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt index 62fe9fc..c6db740 100644 --- a/wpb_home_tutorials/CMakeLists.txt +++ b/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} ) \ No newline at end of file diff --git a/wpb_home_tutorials/src/wpb_home_behavior_node.cpp b/wpb_home_tutorials/src/wpb_home_behavior_node.cpp new file mode 100644 index 0000000..0984a75 --- /dev/null +++ b/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 +#include +#include +#include + +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("/cmd_vel",10); + + ros::spin(); +} diff --git a/wpb_home_tutorials/src/wpb_home_image_node.cpp b/wpb_home_tutorials/src/wpb_home_image_node.cpp new file mode 100644 index 0000000..fef59ac --- /dev/null +++ b/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 +#include +#include +#include +#include + +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; + +} diff --git a/wpb_home_tutorials/src/wpb_home_pointcloud_node.cpp b/wpb_home_tutorials/src/wpb_home_pointcloud_node.cpp new file mode 100644 index 0000000..5a3ed12 --- /dev/null +++ b/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 +#include +#include + +void callbackPC(const sensor_msgs::PointCloud2ConstPtr& msg) +{ + pcl::PointCloud 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; +} \ No newline at end of file