stage simulator

---恢复内容开始---

 运行自带地图

rosrun stage_ros stageros /opt/ros/indigo/share/stage_ros/world/willow-erratic.world 

控制机器人运动
sudo apt-get install ros-indigo-teleop-twist-keyboard
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
简单的gmapping&保存map
rosrun gmapping slam_gmapping scan:=base_scan
rosrun map_server map_saver

在home目录下查看地图

利用激光雷达数据，在前方有障碍物0.5米内停止前行

#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<geometry_msgs/Twist.h>

class Stopper{
public:
    const static double FORWARD_SPEED_MPS = 0.5;
    const static double MIN_SCAN_ANGLE_RAD = -30.0/180*M_PI;
    const static double MAX_SCAN_ANGLE_RAD = +30.0/180*M_PI;
    const static float MIN_PROXOMITY_RANGE_M = 0.5;

    Stopper();
    void startMoving();
private:
    ros::NodeHandle node;
    ros::Publisher commandPub;
    ros::Subscriber laserSub;
    bool keepMoving;
    void moveForward();
    void scanCallback( const sensor_msgs::LaserScan::ConstPtr& scan);
};
Stopper::Stopper(){
    keepMoving = true;
    commandPub = node.advertise<geometry_msgs::Twist>("cmd_vel",10);
    laserSub = node.subscribe("base_scan",1, &Stopper::scanCallback,this);

}

void Stopper::moveForward(){
    geometry_msgs::Twist msg;
    msg.linear.x = FORWARD_SPEED_MPS;
    commandPub.publish(msg);
}

void Stopper::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    int minIndex = ceil((MIN_SCAN_ANGLE_RAD - scan->angle_min) / scan->angle_increment);
    int maxIndex = floor((MAX_SCAN_ANGLE_RAD - scan->angle_min) / 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_PROXOMITY_RANGE_M){
           ROS_INFO("stop");
           keepMoving = false;
       }
    }

}

void Stopper::startMoving(){
ros::Rate rate(10);
ROS_INFO("start_moving");
while(ros::ok()&&keepMoving){
    moveForward();
    ros::spinOnce();
    rate.sleep();
    }
}

int main(int argc,char **argv){
    ros::init(argc,argv,"stopper");
    Stopper stopper;
    stopper.startMoving();
    return 0;

}