参考自Roi Yehoshua 2014PPT
参考激光在指定角度内避障,并且达到最小避障距离后停止。
源代码为:
参考价值:1类的写法;2回调函数转为私有成员;3自定义函数,并在while循环写成构造函数之外另一个函数;
#include "ros/ros.h"
#include "math.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/LaserScan.h"
class stopper
{
public:
const static double FORWARD_SPEED_MPS = 0.5;
const static double MIN_SCAN_ANGEL_RAD =-60/180*M_PI;
const static double MAX_SCAN_ANGEL_RAD = 60/180*M_PI;
const static double MIN_PROXIMITY_RANGE_M = 1.2;
//Shoule be smaller than sensor_msgs::LaserScan::ranger_max
bool keepmoving; //indicates whether the robot should continus moving
ros::NodeHandle node;
ros::Publisher commandpub;
ros::Subscriber lasersub;
stopper()
{
keepmoving = true;
commandpub = node.advertise<geometry_msgs::Twist>("cmd_vel",1000);
lasersub = node.subscribe("scan",1,&stopper::scancallback,this);
}
void moveforward()
{
geometry_msgs::Twist msg;
msg.linear.x = FORWARD_SPEED_MPS;
commandpub.publish(msg);
}
void scancallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
//find the angle area index
int minIndex = ceil((MIN_SCAN_ANGEL_RAD - scan->angle_min)/scan->angle_increment);
int maxIndex = floor((MAX_SCAN_ANGEL_RAD - scan->angle_max)/scan->angle_increment);
float closestRange = scan->ranges[minIndex];
for(int currIndex = minIndex +1; currIndex <= maxIndex;currIndex++)
{
if(scan->ranges[currIndex]< closestRange)
{
closestRange = scan->ranges[currIndex];
}
}
ROS_INFO_STREAM("CLOSEST RANGE: "<< closestRange );
if(closestRange < MIN_PROXIMITY_RANGE_M)
{
ROS_INFO("stop");
keepmoving = false; //modify this public variant!!!! youcan get some data from callback to use for main part
}
}
void startmoving() //it is similar that two thread is running simutanously,main and callbakc
{
ros::Rate rate(10);
ROS_INFO("Start moving");
while(ros::ok() && keepmoving)
{
moveforward();
ros::spinOnce(); //need to call this function often allow ros to process incoming messages
rate.sleep();
}
}
};
int main(int argc,char** argv)
{
ros::init(argc,argv,"stopper");
//bool keepmoving = true;
stopper stp;
stp.startmoving();
return 0;
}
没有将类分开编写,如果声明和实现分离,头文件该怎么写?放在src/pkg/include 下吗?