第九课，ROS仿真1

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

1、stage simulator

它是一个轻量级的仿真软件，它的包名称是stage_ros，可以进入看看，其包含地图在子目录world下，

启动之：

rosrun stage_ros stageros 'rospack find stage_ros'/world/willow-four-erratics-...world

可以查看它发布的主题有哪些，通过rostopic list；

下面查看一下地图的格式：cat willow.erratic.world

其实在world文件中包含下面几个模块

第九课，ROS仿真1-LMLPHP

下面是每一个模型详细的说明：

第九课，ROS仿真1-LMLPHP

下面通过订阅和发布的主题来了解一下该如何使用这款仿真软件

第九课，ROS仿真1-LMLPHP

控制机器人的运动

首先要安装sudo apt-get install ros-indigo-teleop-twist-keyboard

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

简单的保存gmapping以及保存map

下面运行gmapping包下面的slam_gmapping，然后scan订阅的主题是base_scan。

rosrun gmapping slam_gmapping scan:=base_scan

下面语句为保存地图

rosrun map_server map_saver

下面来看一下激光雷达的应用：障碍物的检测

第九课，ROS仿真1-LMLPHP

假设激光雷达的测量范围是270度，上面的橘黄线表示x轴，绿线表示的是y轴，以-135度即range[0]作为起始，把它作为储存距离信息的第一个数，range[1080]作为最后一个。

下面是它的sensor_msgs/LaserScan.msg里面的相关信息，

std_msgs/Header header

float32 angle_min->-135度

float32 angle_max->+135度

float32 angle_increment->指的角度的分辨率，指扫一次角度的变换，为0.25度；

float32 time_increment可以理解为扫一步所花费的时间

float32 scan_time扫面的时间

float32 range_min扫描的最小距离

float32 range_max扫描的最长距离

float32[] ranges储存的是距离的信息

float32[] intensities如果前面有一块玻璃那么强度为1，如果完全为吸收掉则强度为0.

下面用stage_ros来做避障

在讲避障之前，看一下它的tf变换

第九课，ROS仿真1-LMLPHP

运行该节点，且在rviz当中来看其变换

rosrun stage_ros stageros 'rospack find stage_ros'/world/willow-four-erratics-...world

rosrun rviz rviz

将fixed frame选择为odom

然后添加一个地图

第九课，ROS仿真1-LMLPHP

以及

第九课，ROS仿真1-LMLPHP

下面写一个避障的程序，碰到障碍物停下来，订阅主题为/turtlesim/cmd_vel

1、创建一个包

catkin_create_pkg my_stage roscpp std_msgs

2、编译catkin_make

3、源文件名：my_stage.cpp

#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_RAO=-/*M_PI;

    //定义一个最大扫描角度

    const static double MAX_SCAN_ANGLE_RAO=+/*M_PI;

    //声明一个常量，希望的zhang ai wu de 最近距离为0.5米

    const static float MIN_PROXIMITY_RANGE_M=0.5;

    //构造函数

    Stopper();

    //写一个函数   
　　 void startMoving();


　　//声明私有成员
　　ros::NodeHandle node;
　　ros::Publisher commandPub;//用于发布速度信息
　　ros::Subscriber laserSub;//用来订阅base_scan，用来获得激光雷达的数据
　　bool keepMoving;//声明一个布尔类型的指示器
　　void moveForward();//控制机器人向前运动
　　void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);//定义laser_scan的回调函数
};
　　//实现构造函数
　　Stopper::Stopper()
　　{
　　　　//首先把keepMoveing赋值为TRUE
　　　　keepMoving=true;
　　　　commandPub=node.advertise<geometry_msgs::Twist>("cmd_vel",10);//发布消息主题为
　　　　//订阅消息订阅主题为base_scan
　　　　laserSub=node.subscribe("base_scan"，1，&Stopper::scanCallback,this);
　　}
//move_forward函数的实现
　　void Stopper::moveForward()
　　{
　　　　geometry_msgs::Twsit msg;
　　　　msg.linear.x=FORWARD_SPEED_MPS;
　　　　commandPub.publish(msg);//发布这个消息
　　}
//回调函数的实现
void Stopper::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
　　int minIndex,maxIndex;//定义最小以及最大索引
　　minIndex=ceil((MIN_SCAN_ANGLE_RAO-scan->angle_min)/scan->angle_increment);//为指定扫描的最小角度减去激光雷达扫描的最小角度除以增量
　　maxIndex=floor((MAX_SCAN_ANGLE_RAO-scan->angle_min)/scan->angle_increment);
//把最近的距离设为下标为minIndex
　　for(int currIndex=minIndex+1;currIndex<=maxIndex;currIndex++)
　　{
　　　　if(scan->ranges[currIndex]<closestRange)
　　　　{
　　　　　　closestRange=scan->ranges[currIndex];//从minIndex到maxIndex寻找最小距离并赋值给closestRange
　　　　}
　　}
　　//输出最小距离
　　ROS_INFO_STREAM("Closest range:" <<closestRange);
//如果最近的距离比期望的最小距离还要近的话，就输出STOP
　　if(closestRange<MIN_PROXIMITY_RANGE_M)
　　{
　　　　ROS_INFO("Stop!");
　　　　keepMoving=false;
　　}
}
//StopMoving函数
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;
}

下面进入CMakeLists.txt文件里面去修改
add_executable(my_stage src/my_stage.cpp)

target_Link_libraries(my_stage ${catkin_LIBRARIES})

下面去编译

catkin_make;

下面去启动stage仿真器

rosrun stage_ros stageros `rospack find stage_ros`/world/willow-erratic.world

运行之：

rosrun my_stage my_stage

它是一直往前走直到遇到障碍物之后就停止下来。

---恢复内容结束---

1、stage simulator

它是一个轻量级的仿真软件，它的包名称是stage_ros，可以进入看看，其包含地图在子目录world下，

启动之：

rosrun stage_ros stageros 'rospack find stage_ros'/world/willow-four-erratics-...world

可以查看它发布的主题有哪些，通过rostopic list；

下面查看一下地图的格式：cat willow.erratic.world

其实在world文件中包含下面几个模块

第九课，ROS仿真1-LMLPHP

下面是每一个模型详细的说明：

第九课，ROS仿真1-LMLPHP

下面通过订阅和发布的主题来了解一下该如何使用这款仿真软件

第九课，ROS仿真1-LMLPHP

控制机器人的运动

首先要安装sudo apt-get install ros-indigo-teleop-twist-keyboard

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

简单的保存gmapping以及保存map

下面运行gmapping包下面的slam_gmapping，然后scan订阅的主题是base_scan。

rosrun gmapping slam_gmapping scan:=base_scan

下面语句为保存地图

rosrun map_server map_saver

下面来看一下激光雷达的应用：障碍物的检测

第九课，ROS仿真1-LMLPHP

下面是它的sensor_msgs/LaserScan.msg里面的相关信息，

std_msgs/Header header

float32 angle_min->-135度

float32 angle_max->+135度

float32 angle_increment->指的角度的分辨率，指扫一次角度的变换，为0.25度；

float32 time_increment可以理解为扫一步所花费的时间

float32 scan_time扫面的时间

float32 range_min扫描的最小距离

float32 range_max扫描的最长距离

float32[] ranges储存的是距离的信息

float32[] intensities如果前面有一块玻璃那么强度为1，如果完全为吸收掉则强度为0.

下面用stage_ros来做避障

在讲避障之前，看一下它的tf变换

第九课，ROS仿真1-LMLPHP

运行该节点，且在rviz当中来看其变换

rosrun stage_ros stageros 'rospack find stage_ros'/world/willow-four-erratics-...world

rosrun rviz rviz

将fixed frame选择为odom

然后添加一个地图

第九课，ROS仿真1-LMLPHP

以及

第九课，ROS仿真1-LMLPHP

下面写一个避障的程序，碰到障碍物停下来，订阅主题为/turtlesim/cmd_vel

1、创建一个包

catkin_create_pkg my_stage roscpp std_msgs

2、编译catkin_make

3、源文件名：my_stage.cpp

#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_RAO=-/*M_PI;

    //定义一个最大扫描角度

    const static double MAX_SCAN_ANGLE_RAO=+/*M_PI;

    //声明一个常量，希望的zhang ai wu de 最近距离为0.5米

    const static float MIN_PROXIMITY_RANGE_M=0.5;

    //构造函数

    Stopper();

    //写一个函数   
　　 void startMoving();


　　//声明私有成员
　　ros::NodeHandle node;
　　ros::Publisher commandPub;//用于发布速度信息
　　ros::Subscriber laserSub;//用来订阅base_scan，用来获得激光雷达的数据
　　bool keepMoving;//声明一个布尔类型的指示器
　　void moveForward();//控制机器人向前运动
　　void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);//定义laser_scan的回调函数
};
　　//实现构造函数
　　Stopper::Stopper()
　　{
　　　　//首先把keepMoveing赋值为TRUE
　　　　keepMoving=true;
　　　　commandPub=node.advertise<geometry_msgs::Twist>("cmd_vel",10);//发布消息主题为
　　　　//订阅消息订阅主题为base_scan
　　　　laserSub=node.subscribe("base_scan"，1，&Stopper::scanCallback,this);
　　}
//move_forward函数的实现
　　void Stopper::moveForward()
　　{
　　　　geometry_msgs::Twsit msg;
　　　　msg.linear.x=FORWARD_SPEED_MPS;
　　　　commandPub.publish(msg);//发布这个消息
　　}
//回调函数的实现
void Stopper::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan)
{
　　int minIndex,maxIndex;//定义最小以及最大索引
　　minIndex=ceil((MIN_SCAN_ANGLE_RAO-scan->angle_min)/scan->angle_increment);//为指定扫描的最小角度减去激光雷达扫描的最小角度除以增量
　　maxIndex=floor((MAX_SCAN_ANGLE_RAO-scan->angle_min)/scan->angle_increment);
//把最近的距离设为下标为minIndex
　　for(int currIndex=minIndex+1;currIndex<=maxIndex;currIndex++)
　　{
　　　　if(scan->ranges[currIndex]<closestRange)
　　　　{
　　　　　　closestRange=scan->ranges[currIndex];//从minIndex到maxIndex寻找最小距离并赋值给closestRange
　　　　}
　　}
　　//输出最小距离
　　ROS_INFO_STREAM("Closest range:" <<closestRange);
//如果最近的距离比期望的最小距离还要近的话，就输出STOP
　　if(closestRange<MIN_PROXIMITY_RANGE_M)
　　{
　　　　ROS_INFO("Stop!");
　　　　keepMoving=false;
　　}
}
//StopMoving函数
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;
}

下面进入CMakeLists.txt文件里面去修改
add_executable(my_stage src/my_stage.cpp)

target_Link_libraries(my_stage ${catkin_LIBRARIES})

下面去编译

catkin_make;

下面去启动stage仿真器

rosrun stage_ros stageros `rospack find stage_ros`/world/willow-erratic.world

运行之：

rosrun my_stage my_stage

它是一直往前走直到遇到障碍物之后就停止下来。