laser 避障

参考自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 下吗？