ROS入门--基于激光的避障

Task

本次任务的人物是编写一个简单的ROS应用，基于Braitenberg 小车，我们只做stage仿真,如果你有ros的移动机器人，可以在通过修改代码在自己的机器上运行。

Algorithm

我们用的机器人上面有一个激光雷达,激光雷达可以测量障碍物到机器人的距离，根据这些测距信息来设置机器人的左右轮子的旋转速度。通过ROS发送线速度和角速度。

Processing inputs and outputs

类NodeBraitenberg2里面除去构造、析构函数、还有两个方法：一个是laser数据处理函数 一个是机器人控制程序。
激光雷达发布的消息格式为：LaserScan,包含有数据：

std_msgs/Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities 

float32[] ranges 里面存放的是激光的原始数据。激光雷安装在小车的中轴线上，ranges的前半部分和后半部分分别对应着机器人的左侧和右侧，通过下面公式可以计算出对ranges里面索引对应的实际角度：

φ=(i−l2)⋅θ(angle-ranges)

phai对应的是i个索引对应的角度，l是ranges的长度，theta是角度增量（angle_increment）
发送给机器人的Twist消息：

geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

Conversion

将机器人设定为恒定速度运动，角速度计算：

|ω|=c⋅dlds−c(angular-velocity)

• |ω| ：角速度的绝对值
• c ：常数，最小距离比率的敏感度
• dl : 激光数据的最大值
• ds : 激光数据的最小值

通过上面的公式可以获得机器人角速度的绝对值，然后可以通过if/else选择机器人左拐还是右拐

程序编写

创建package和文件

跳转到intro_ros_ws/src目录下面（intro_ros_ws是我的ros 空间，自己可以新建一个,如下：）

1. mkdir -p your_sapce_name/src
2. cd your_sapce_name/src
3. catkin_init_workspace

创建dem_braitenberg package：
catkin_create_pkg dem_braitenberg roscpp rospy std_msgs

新建文件来操作机器人：

• include/node_braitenberg.h
• src/node_braitenberg.cpp

新建一个文件存放main函数：

• src/braitenberg.cpp

修改CmakeLists.txt

在CmakeLists.txt(dem_braitenberg目录下面)末尾添加：
set (SRCS1 ${SRCS1} src/node_braitenberg.cpp)
set (SRCS1 ${SRCS1} src/braitenberg.cpp)
添加catkin目录：
include_directories(include ${catkin_INCLUDE_DIRS}
创建可执行文件和链接catkin库：
add_executable(braitenberg ${SRCS1})
target_link_libraries(braitenberg ${catkin_LIBRARIES})

现在可以开始编写代码了。

Code： node_braitenberg.h

#ifndef SR_NODE_BRAITENBERG2
#define SR_NODE_BRAITENBERG2

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/Twist.h"

/* Demonstration task: "Breitenberg Vehicle"
 *
 * This class controls robot. It's behavior is inspired by Breitenberg's 
 * vehicle. In this case robot find minimal value on the left and right 
 * side and goes, where the value is higher.
 */

class NodeBraitenberg2
{
public:

  /* Constructor:
   * 
   * pub    Publisher, which can send commands to robot.
   * angleC Value, which will be stored in angleCoef.
   * speed     Value, which will be stored in robotSpeed.
   */

  NodeBraitenberg2(ros::Publisher pub, double angleC, double speed);

  ~NodeBraitenberg2();

  /* This method reads data from sensor and processes them to variables.
   * 
   * This method finds minimal distances on the left and right side 
   * and saves them to variables distMinLeft, distMinRight.
   * 
   * @param msg Message, which came from robot and contains data from
   * laser scan.
   */

  void messageCallback(const sensor_msgs::LaserScan::ConstPtr& msg);

private:

  /* This method publishes commands for robot.
   *
   * Commands are generated from data, which are stored in variables
   * (distMinLeft, distMinRight). Robot turns to direction, which has higher 
   * value. Robot turns sharper, if higher value >> lower value.
   */

  void publishMessage();

  // Variables

  double angleCoef;       // Coeficient for transfering angles to speed.
  double robotSpeed;        // Speed of robot [m/s].
  double angleMinLeft;       // Angle, at which was measured the shortest distance on the left.
  double distMinLeft;        // Minimum distance masured by sensor on the left.
  double angleMinRight;      // Angle, at which was measured the shortest distance on the right.
  double distMinRight;       // Minimum distance masured by sensor on the right.
  ros::Publisher pubMessage; // Object for publishing messages.
};

#endif

Code： node_braitenberg.cpp

#include "node_braitenberg.h"

//Constructor and destructor
NodeBraitenberg2::NodeBraitenberg2(ros::Publisher pub, double angleC, double speed)
{
  angleCoef = angleC;
  robotSpeed = speed;
  pubMessage = pub;
}

NodeBraitenberg2::~NodeBraitenberg2()
{
}

//Publisher
void NodeBraitenberg2::publishMessage()
{
  //preparing message
  geometry_msgs::Twist msg;

  if (distMinLeft >= distMinRight)
  {
    msg.angular.z = angleCoef*distMinLeft/distMinRight-angleCoef;
  }
  else
  {
    msg.angular.z = -(angleCoef*distMinRight/distMinLeft-angleCoef);
  }

  msg.linear.x = robotSpeed;
  if (distMinLeft < 0.25 && distMinRight < 0.25 && angleMinLeft < 0.7 && angleMinRight < 0.7)
  {
    msg.angular.z*=50;
    msg.linear.x*=0.5;
  }

  //publishing message
  pubMessage.publish(msg);
}

//Subscriber
void NodeBraitenberg2::messageCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  //Calculation of array size from angle range and angle increment.
  int size = msg->ranges.size();
  int minIndexLeft = 0;
  int minIndexRight = size/2;
  //This cycle goes through array and finds minimum on the left and right
  for(int i=0; i<size/2; i++)
  {
    if (msg->ranges[i] < msg->ranges[minIndexRight] && msg->ranges[i] > 0.05){
      minIndexRight = i;
    } 
  }
  for (int i = size/2; i <size ; i++)
  {
    if (msg->ranges[i] < msg->ranges[minIndexLeft] && msg->ranges[i] > 0.05){
      minIndexLeft = i;
    } 
  }

  //Calculation of angle from indexes and storing data to class variables.
  angleMinLeft = (minIndexLeft-size/2)*msg->angle_increment;
  distMinLeft = msg->ranges[minIndexLeft];
  angleMinRight = (minIndexRight-size/2)*msg->angle_increment;
  distMinRight = msg->ranges[minIndexRight];

  //Invoking method for publishing message
  publishMessage();
}

Code: braitenberg.cpp

#include "node_braitenberg.h"

#define SUBSCRIBER_BUFFER_SIZE 1  // Size of buffer for subscriber.
#define PUBLISHER_BUFFER_SIZE 1000  // Size of buffer for publisher.
#define ANGLE_COEF 2 // Coeficient for angles.
#define ROBOT_SPEED 0.10 // Speed of robot [m/s].
// #define PUBLISHER_TOPIC "/syros/base_cmd_vel"
#define PUBLISHER_TOPIC "/cmd_vel"
// #define SUBSCRIBER_TOPIC "/syros/laser_laser"
#define SUBSCRIBER_TOPIC "/base_scan"

/* task: "Braitenberg vehicle 2"
 * 
 * In main function is created Subscribing node, which transmits messages 
 * to NodeBraitenberg2 object. There are the messages processed and commands 
 * generated.
 */

int main(int argc, char **argv)
{
  //Initialization of node
  ros::init(argc, argv, "braitenberg2");
  ros::NodeHandle n;

  //Creating publisher
  ros::Publisher pubMessage = n.advertise<geometry_msgs::Twist>(PUBLISHER_TOPIC, PUBLISHER_BUFFER_SIZE);

  //Creating object, which stores data from sensors and has methods for
  //publishing and subscribing
  NodeBraitenberg2 *nodeBraitenberg2 = new NodeBraitenberg2(pubMessage, ANGLE_COEF, ROBOT_SPEED);

  //Creating subscriber
  ros::Subscriber sub = n.subscribe(SUBSCRIBER_TOPIC, SUBSCRIBER_BUFFER_SIZE, &NodeBraitenberg2::messageCallback, nodeBraitenberg2);
  ros::spin();

  return 0;
}

编译

跳转到intro_ros_ws目录下面，执行指令：

catkin_make

实验

第一步运行roscore:

roscore

运行仿真环境：

rosrun stage stageros your_file.world

运行避障程序：

rosrun dem_braitenberg braitenberg

译者：代码封装不完整，仅供参考，我的代码：

https://github.com/will1991/rosintrodution