当前位置：首页 > news >正文

TurtleBot3 Package turtlebot3_drive source code read

news 来源：原创 2025/9/19 2:22:18

前言

此处阅读简单的 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     3class Turtlebot3Drive
{public:Turtlebot3Drive();~Turtlebot3Drive();bool init();bool controlLoop();private:// ROS NodeHandleros::NodeHandle nh_;ros::NodeHandle nh_priv_;    // not see your funciton here, maybe for further extension// ROS Parameters// ROS Time// ROS Topic Publishersros::Publisher cmd_vel_pub_;// ROS Topic Subscribersros::Subscriber laser_scan_sub_;ros::Subscriber odom_sub_;// Variablesdouble 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 prototypesvoid 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 nodeROS_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 parameterstd::string cmd_vel_topic_name = nh_.param<std::string>("cmd_vel_topic_name", "");// initialize variablesescape_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 publisherscmd_vel_pub_   = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic_name, 10);// initialize subscriberslaser_scan_sub_  = nh_.subscribe("scan", 10, &Turtlebot3Drive::laserScanMsgCallBack, this);  // asynchronous, callback funcitonodom_sub_ = nh_.subscribe("odom", 10, &Turtlebot3Drive::odomMsgCallBack, this);    // the topic name i have seenreturn 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 thenupdatecommandVelocity(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;elseupdatecommandVelocity(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;elseupdatecommandVelocity(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得知，它无法单独完成工作，必须和其他节点配合。

前言

turtlebot3_drive.h

turtlebot3_drive.cpp

attention

Summerize

相关文章：