ROS STAGE教程3 （编译源码，自定义Lidar噪声）Ubuntu 18.04 Ubuntu 16.04

源码地址：https://github.com/rtv/Stage.git

系统：Ubuntu 18.04 ROS Melodic

噪声生成模块 lasernoise 路径为Stage\examples\ctrl\lasernoise.cc

#include "stage.hh"
using namespace Stg;

const double DEVIATION = 0.05;

double simple_normal_deviate(double mean, double stddev)
{
  double x = 0.0;

  for (int i = 0; i < 12; i++)
    x += rand() / (RAND_MAX + 1.0);

  return (stddev * (x - 6.0) + mean);
}

// process the ranger data
int RangerUpdate(ModelRanger *mod, void *)
{
  // get the data
  std::vector<meters_t> &scan = mod->GetSensorsMutable()[0].ranges;

  if (scan.size() > 0)
    FOR_EACH (it, scan)
      *it *= simple_normal_deviate(1.0, DEVIATION);

  return 0; // run again
}

// Stage calls this when the model starts up. we just add a callback to
// the model that gets called just after the sensor update is done.
extern "C" int Init(Model *mod)
{
  mod->AddCallback(Model::CB_UPDATE, (model_callback_t)RangerUpdate, NULL);

  // add this so we can see the effects immediately, without needing
  // anyone else to subscribe to the ranger
  mod->Subscribe();

  return 0; // ok
}

可以看到标准差 定义为 0.05 可以自己调整

按照指令安装相关依赖包：

sudo apt-get install git cmake g++ fltk1.1-dev libjpeg8-dev libglu1-mesa-dev libltdl-dev	

提前删除原路径下(/opt/ros/melodic/lib/Stage-4.3)文件：sudo rm lasernoise.so 

按照以下指令生成动态库文件 lasernoise.so（注意此时STAGE版本为4.3，不能直接用于ROS-kinetic替换）

$ mkdir stage4
$ cd stage4
$ git clone git://github.com/rtv/Stage.git
$ export STG=/opt/ros/kinetic
$ cmake -DCMAKE_INSTALL_PREFIX=$STG Stage
$ make
$ sudo make install

系统：Ubuntu 16.04 ROS kinetic

下载Stage-4.1.1压缩包地址：https://github.com/rtv/Stage/releases/tag/v4.1.1 并解压缩到stage4文件夹中

将Stage-4.1.1/cmake/internal/FindOS.cmake文件中lib64改为lib，否则安装路径不是/opt/ros/kinetic/lib/Stage-4.1

IF(PROJECT_PROC_64BIT)
    # Set the install path to lib64
    SET(PROJECT_LIB_DIR "lib")
    SET(PROJECT_PLUGIN_DIR "lib/${PROJECT_NAME}-${APIVERSION}")
ELSE (PROJECT_PROC_64BIT)
    SET(PROJECT_LIB_DIR "lib")
    SET(PROJECT_PLUGIN_DIR "lib/${PROJECT_NAME}-${APIVERSION}")
ENDIF (PROJECT_PROC_64BIT)

默认噪声标准公差0.05

预先将该路径下的lasernoise.so删除（或者直接sudo make uninstall），否则安装无法替换！

stage4路径下执行以下命令

export STG=/opt/ros/kinetic
cmake -DCMAKE_INSTALL_PREFIX=$STG Stage-4.1.1
make
sudo make install

此时再调用之前rosrun或者roslaunch就可以看到噪声降低了