ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update lidar_behavior@tutorials

Browse Source
This commit is contained in:
zhangwanjie 2020-05-05 12:25:53 +08:00
parent a1deaa5248
commit 4450411206
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
wpb_home_tutorials/src/wpb_home_lidar_behavior.cpp
View File

@ -41,6 +41,7 @@
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
ros::Publisher vel_pub; ros::Publisher vel_pub;
static int nCount = 0;
void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan) void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{ {
@ -50,14 +51,21 @@ void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
float fMidDist = scan->ranges[nMid]; float fMidDist = scan->ranges[nMid];
ROS_INFO("Point[%d] = %f", nMid, fMidDist); ROS_INFO("Point[%d] = %f", nMid, fMidDist);
if(nCount > 0)
{
nCount--;
return;
}
geometry_msgs::Twist vel_cmd; geometry_msgs::Twist vel_cmd;
if(fMidDist > 1.0f) if(fMidDist > 1.5f)
{ {
vel_cmd.linear.x = 0.05; vel_cmd.linear.x = 0.05;
} }
else else
{ {
vel_cmd.angular.z = 0.1; vel_cmd.angular.z = 0.3;
nCount = 50;
} }
vel_pub.publish(vel_cmd); vel_pub.publish(vel_cmd);
} }