From 4893b6832f55ecdb5ef084f6046e4cd3fc0c1d5f Mon Sep 17 00:00:00 2001 From: zhangwanjie Date: Sat, 18 Jul 2020 16:47:46 +0800 Subject: [PATCH] new lidar demo --- wpb_home_tutorials/CMakeLists.txt | 8 ++ wpb_home_tutorials/launch/lidar_demo.launch | 31 +++++ .../src/wpb_home_lidar_obstacle.cpp | 125 ++++++++++++++++++ 3 files changed, 164 insertions(+) create mode 100644 wpb_home_tutorials/launch/lidar_demo.launch create mode 100644 wpb_home_tutorials/src/wpb_home_lidar_obstacle.cpp diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt index f67f0f8..2afc34e 100644 --- a/wpb_home_tutorials/CMakeLists.txt +++ b/wpb_home_tutorials/CMakeLists.txt @@ -326,6 +326,14 @@ target_link_libraries(wpb_home_lidar_behavior ${catkin_LIBRARIES} ) +add_executable(wpb_home_lidar_obstacle + src/wpb_home_lidar_obstacle.cpp +) +add_dependencies(wpb_home_lidar_obstacle ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(wpb_home_lidar_obstacle + ${catkin_LIBRARIES} +) + add_executable(wpb_home_grab_object src/wpb_home_grab_object.cpp ) diff --git a/wpb_home_tutorials/launch/lidar_demo.launch b/wpb_home_tutorials/launch/lidar_demo.launch new file mode 100644 index 0000000..dc01bbf --- /dev/null +++ b/wpb_home_tutorials/launch/lidar_demo.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wpb_home_tutorials/src/wpb_home_lidar_obstacle.cpp b/wpb_home_tutorials/src/wpb_home_lidar_obstacle.cpp new file mode 100644 index 0000000..0b70e87 --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_lidar_obstacle.cpp @@ -0,0 +1,125 @@ +/********************************************************************* +* 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; + +// 用来计算激光雷达检测点(x,y)坐标的变量 +static double x_cos[360]; +static double y_sin[360]; +static float pnt_x[360]; +static float pnt_y[360]; + +void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan) +{ + // 将所有测距值解算成(x,y)坐标值 + for(int i=0;i<360;i++) + { + pnt_x[i] = scan->ranges[i] * x_cos[i]; + pnt_y[i] = scan->ranges[i] * y_sin[i]; + } + + // 对90度到270度的障碍点取正前方最近的点 + float min_x = 999.99; + int min_index = 180; + for(int i=90;i<270;i++) + { + // 在机器人中轴线左右0.4米范围内的障碍点才判断其正向距离值 + if(fabs(pnt_y[i])<0.4) + { + if(pnt_x[i] < min_x) + { + // 记录正向距离值最小的障碍点角度 + min_x = pnt_x[i]; + min_index = i; + } + } + } + ROS_WARN("min_x= %.2f min_y = %.2f min_index= %d",min_x,pnt_y[min_index],min_index); + + // 对正向距离值最小的点进行判断，是否需要避障 + geometry_msgs::Twist vel_cmd; + if(min_x < 1.0) + { + // 正向距离小于1.0米，需要避障 + vel_cmd.linear.x = 0; + if(min_index < 180) + { + // 障碍物在右前方，向左平移避开 + vel_cmd.linear.y = 0.05; + } + else + { + // 障碍物在左前方，向右平移避开 + vel_cmd.linear.y = -0.05; + } + } + else + { + // 正向距离大于1.0米，不需要避障 + vel_cmd.linear.x = 0.05; + vel_cmd.linear.y = 0; + } + vel_pub.publish(vel_cmd); +} + +int main(int argc, char** argv) +{ + ros::init(argc,argv,"wpb_home_lidar_obstacle"); + + ROS_INFO("wpb_home_lidar_obstacle start!"); + + // 初始化正余弦值，后面用来解算障碍点的（x，y）坐标 + double kStep = (M_PI * 2) / 360; + for (int i = 0; i < 360; i++) + { + x_cos[i] = -1*cos(M_PI*0.0 - kStep*i); + y_sin[i] = -1*sin(M_PI*0.0 - kStep*i); + } + + // 订阅激光雷达主题，发布速度控制主题 + ros::NodeHandle nh; + ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback); + vel_pub = nh.advertise("/cmd_vel",10); + + ros::spin(); +}