TurtleBot3 Package turtlebot3_drive source code read

前言

此处阅读简单的 turtlebot3_drive 代码。

从ROS的角度，作为一个demo，它足够小、简单，可以从中看见ROS的 NodeHandle如何使用。此外，我们也能简单地看到 “自动避障功能的实现”。

从C++的角度，它实际上并不复杂，我们能够回顾C++编码的规范，对于初学者有帮助。

turtlebot3_drive.h 

/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Taehun Lim (Darby) */

#ifndef TURTLEBOT3_DRIVE_H_
#define TURTLEBOT3_DRIVE_H_

#include <ros/ros.h>

#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

#define DEG2RAD (M_PI / 180.0)
#define RAD2DEG (180.0 / M_PI)

#define CENTER 0
#define LEFT   1
#define RIGHT  2

#define LINEAR_VELOCITY  0.3
#define ANGULAR_VELOCITY 1.5

#define GET_TB3_DIRECTION 0
#define TB3_DRIVE_FORWARD 1
#define TB3_RIGHT_TURN    2
#define TB3_LEFT_TURN     3

class Turtlebot3Drive
{
 public:
  Turtlebot3Drive();
  ~Turtlebot3Drive();
  bool init();
  bool controlLoop();

 private:
  // ROS NodeHandle
  ros::NodeHandle nh_;
  ros::NodeHandle nh_priv_;    // not see your funciton here, maybe for further extension

  // ROS Parameters

  // ROS Time

  // ROS Topic Publishers
  ros::Publisher cmd_vel_pub_;

  // ROS Topic Subscribers
  ros::Subscriber laser_scan_sub_;
  ros::Subscriber odom_sub_;

  // Variables
  double escape_range_;
  double check_forward_dist_;
  double check_side_dist_;

  double scan_data_[3] = {0.0, 0.0, 0.0};

  double tb3_pose_;
  double prev_tb3_pose_;    // 2 variables to keep track

  // Function prototypes
  void updatecommandVelocity(double linear, double angular);
  void laserScanMsgCallBack(const sensor_msgs::LaserScan::ConstPtr &msg);
  void odomMsgCallBack(const nav_msgs::Odometry::ConstPtr &msg);
};
#endif // TURTLEBOT3_DRIVE_H_

一般而言，我们先看 .h 文件，了解接口。在文件的最上方，是协议。此外，我们看到了头文件的包含，#define 定义的宏。这些宏通常作为常量，方便编码，提高可读性。

在class内部，有两个接口，init() 和 controlLoop()。其中 init()负责初始化工作，controlLoop()是控制循环，于是我们知道，实际的实现文件中，可能存在一个循环，其中起作用的就是controlLoop()方法。

在 private部分，定义了变量，有注释进行说明。最后，还有三个 helper function/ utility function，用于辅助public函数功能的实现，但对外隐蔽，提高了封装性。此外有两个变量记录着 turtle的姿态。

turtlebot3_drive.cpp

/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Taehun Lim (Darby) */

#include "turtlebot3_gazebo/turtlebot3_drive.h"

Turtlebot3Drive::Turtlebot3Drive()
  : nh_priv_("~")
{
  //Init gazebo ros turtlebot3 node
  ROS_INFO("TurtleBot3 Simulation Node Init");
  auto ret = init();
  ROS_ASSERT(ret);
}

Turtlebot3Drive::~Turtlebot3Drive()
{
  updatecommandVelocity(0.0, 0.0);
  ros::shutdown();
}

/*******************************************************************************
* Init function
*******************************************************************************/
bool Turtlebot3Drive::init()
{
  // initialize ROS parameter
  std::string cmd_vel_topic_name = nh_.param<std::string>("cmd_vel_topic_name", "");

  // initialize variables
  escape_range_       = 30.0 * DEG2RAD;
  check_forward_dist_ = 0.7;
  check_side_dist_    = 0.6;

  tb3_pose_ = 0.0;                 // so there is only 2 kind of pose recorded 
  prev_tb3_pose_ = 0.0;     // record the previous pose   

  // initialize publishers
  cmd_vel_pub_   = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic_name, 10);

  // initialize subscribers
  laser_scan_sub_  = nh_.subscribe("scan", 10, &Turtlebot3Drive::laserScanMsgCallBack, this);  // asynchronous, callback funciton
  odom_sub_ = nh_.subscribe("odom", 10, &Turtlebot3Drive::odomMsgCallBack, this);    // the topic name i have seen

  return true;
}

void Turtlebot3Drive::odomMsgCallBack(const nav_msgs::Odometry::ConstPtr &msg)
{
  double siny = 2.0 * (msg->pose.pose.orientation.w * msg->pose.pose.orientation.z + msg->pose.pose.orientation.x * msg->pose.pose.orientation.y);
	double cosy = 1.0 - 2.0 * (msg->pose.pose.orientation.y * msg->pose.pose.orientation.y + msg->pose.pose.orientation.z * msg->pose.pose.orientation.z);  

	tb3_pose_ = atan2(siny, cosy);
}

void Turtlebot3Drive::laserScanMsgCallBack(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  uint16_t scan_angle[3] = {0, 30, 330};

  for (int num = 0; num < 3; num++)
  {
    if (std::isinf(msg->ranges.at(scan_angle[num])))
    {
      scan_data_[num] = msg->range_max;
    }
    else
    {
      scan_data_[num] = msg->ranges.at(scan_angle[num]);
    }
  }
}

void Turtlebot3Drive::updatecommandVelocity(double linear, double angular)
{
  geometry_msgs::Twist cmd_vel;

  cmd_vel.linear.x  = linear;
  cmd_vel.angular.z = angular;

  cmd_vel_pub_.publish(cmd_vel);
}

/*******************************************************************************
* Control Loop function
*******************************************************************************/
bool Turtlebot3Drive::controlLoop()
{
  static uint8_t turtlebot3_state_num = 0;

  switch(turtlebot3_state_num)
  {
    case GET_TB3_DIRECTION:
      if (scan_data_[CENTER] > check_forward_dist_)
      {
        if (scan_data_[LEFT] < check_side_dist_)
        {
          prev_tb3_pose_ = tb3_pose_;
          turtlebot3_state_num = TB3_RIGHT_TURN;
        }
        else if (scan_data_[RIGHT] < check_side_dist_)
        {
          prev_tb3_pose_ = tb3_pose_;
          turtlebot3_state_num = TB3_LEFT_TURN;
        }
        else
        {
          turtlebot3_state_num = TB3_DRIVE_FORWARD;
        }
      }

      if (scan_data_[CENTER] < check_forward_dist_)
      {
        prev_tb3_pose_ = tb3_pose_;
        turtlebot3_state_num = TB3_RIGHT_TURN;    // default, turn right when there is an obstacle
      }
      break;

    case TB3_DRIVE_FORWARD:    // your logic is the same,  after update the velocity,  switch to get info then
      updatecommandVelocity(LINEAR_VELOCITY, 0.0);
      turtlebot3_state_num = GET_TB3_DIRECTION;
      break;

    case TB3_RIGHT_TURN:
      if (fabs(prev_tb3_pose_ - tb3_pose_) >= escape_range_)
        turtlebot3_state_num = GET_TB3_DIRECTION;
      else
        updatecommandVelocity(0.0, -1 * ANGULAR_VELOCITY);
      break;

    case TB3_LEFT_TURN:
      if (fabs(prev_tb3_pose_ - tb3_pose_) >= escape_range_)
        turtlebot3_state_num = GET_TB3_DIRECTION;
      else
        updatecommandVelocity(0.0, ANGULAR_VELOCITY);
      break;

    default:
      turtlebot3_state_num = GET_TB3_DIRECTION;
      break;
  }

  return true;
}

/*******************************************************************************
* Main function
*******************************************************************************/
int main(int argc, char* argv[])
{
  ros::init(argc, argv, "turtlebot3_drive");
  Turtlebot3Drive turtlebot3_drive;

  ros::Rate loop_rate(125);

  while (ros::ok())
  {
    turtlebot3_drive.controlLoop();
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

cstr 输出一份 ROS log，将具体的工作交由 init()方法实现，并由assert()判断实现是否成功初始化。

init()内部对几个 private member进行了初始化，并通过 Nodehandle给 ROS的sub、pub变量声明了相关的 topic，queue size。对于 sub，它还声明了回调函数，注意到 sub 的后两个参数，一个提供了类方法的地址，另一个是 指针 this，指明该类自身。这是C++回调函数的典型写法，原因显而易见：需要给出哪个类的哪个方法，最后的 this 指明该类的 “此对象”。

controlLoop()实际上是一个 switch语句，其中有各种case标识，无非是标识几种状态，在对应的状态中，对laser读得的数据进行判断，并执行各种操作，实现自主前行，自动避障。注意到，雷达存在扫描范围，所以在方法 laserScanMsgCallBack(),  我们对接收到的距离做了判断，并根据情况赋予不同的值。

注意成员函数odomMsgCallBack() 会改变 turtle的姿态，会影响controlLoop()的条件判断和具体语句执行。updatecommandVelocity则改变速度。

attention

这里存在几个细节：

1、controlloop()中，初始化了一个 static 变量。它的生存期从该函数开始执行到节点结束，作用域在函数内部。static还保证了第二次调用该函数时，它不会反复初始化，符合我们的期待。放入函数内部表明，它实际只跟函数相关。

2、main()函数中，使用了一个 while循环，设定了 ros::rate()，定期唤醒当前节点，一种典型的 ros sub端的处理方式。

3、对语句

std::isinf(msg->ranges.at(scan_angle[num]))

 std::isinf() 是 STL的部分，msg为ROS的一种数据格式。ranges实际为被包括的 std::vector<float>，详见 ROS Message Type, at()是 std::vector<>的一种方法，比 [] 要安全，如果索引超出范围，它抛出异常。

Summerize

上述的代码很简单，没有复杂的逻辑。可以从订阅topic和发布topic得知，它无法单独完成工作，必须和其他节点配合。