kobuki 源码阅读

目  录

第1章 kobuki源码 2

1.1 kobuki协议的网址： 2

1.1.1 协议相关接口 2

1.1.2 串口通信的核心代码： 2

第2章 Move_base源码 3

第3章 Nodelets 4

3.1 Bumper2PcNodelet 4

3.2 SafetyControllerNodelet代码关系 5

3.3 键盘控制 6

3.4 话题关系 6

3.4.1 sensors/core 6

3.5 BumpBlinkControllerNodelet 6

3.6 KobukiNodelet 6

3.7 RandomWalkerControllerNodelet 7

第1章 kobuki源码

1.1 kobuki协议的网址：

Protocol Specification

http://docs.ros.org/kinetic/api/kobuki_driver/html/enAppendixProtocolSpecification.html

1.1.1 协议相关接口

类名PacketFinderBase，class PacketFinder : public PacketFinderBase，

kobuki-kinetic\kobuki_core-kinetic\kobuki_driver\src\driver\kobuki.cpp

kobuki-kinetic\kobuki_core-kinetic\kobuki_driver\include\kobuki_driver\packet_handler\packet_finder.hpp

bool PacketFinder::checkSum()

bool serialise(ecl::PushAndPop<unsigned char> & byteStream)

bool deserialise(ecl::PushAndPop<unsigned char> & byteStream)

1.1.2 串口通信的核心代码：

类名Kobuki

kobuki-kinetic\kobuki_core-kinetic\kobuki_driver\src\driver\kobuki.cpp

ecl::Serial serial;

 

serial.open(parameters.device_port, ecl::BaudRate_115200, ecl::DataBits_8, ecl::StopBits_1, ecl::NoParity);

void Kobuki::sendCommand(Command command)

serial.write((const char*)&command_buffer[0], command_buffer.size());

void Kobuki::spin()

int n = serial.read((char*)buf, packet_finder.numberOfDataToRead());

 

Action ,Goal feedback result运行过程：

AutoDockingNodelet->AutoDockingROS

 

Cmd_vel运行过程：

KobukiNodelet->KobukiRos->Kobuki subscriber publisher串口通信.

 

 

 

 

第2章 Move_base源码

Topic 命名映射：

turtlebot-kinetic\turtlebot_bringup\launch\includes\create\mobile_base.launch.xml

  <remap from="cmd_vel" to="mobile_base/commands/velocity" />

Master_api.py注册了很多move_base相关的topic

 

Teleop.py

self.cmd_vel_subscriber = rospy.Subscriber("cmd_vel", geometry_msgs.Twist, self.ros_cmd_vel_callback)

 

Cmd_vel:

velocity_command_subscriber = nh.subscribe(std::string("commands/velocity"), 10, &KobukiRos::subscribeVelocityCommand,this);

kobuki.setBaseControl(msg->linear.x, msg->angular.z);// Kobuki kobuki;

diff_drive.setVelocityCommands(linear_velocity, angular_velocity);//DiffDrive diff_drive;

void DiffDrive::setVelocityCommands(const double &vx,const double &wz) {

  // vx: in m/s

  // wz: in rad/s

  std::vector<double> cmd_vel;

  cmd_vel.push_back(vx);

  cmd_vel.push_back(wz);

  point_velocity = cmd_vel;

}

 

 

Kobuki::spin()

 sendBaseControlCommand();

void Kobuki::sendCommand(Command command)

 

第3章 Nodelets

共有6个nodelets

Kobuki_bumper2pc.hpp (ork\src\kobuki-kinetic\kobuki_bumper2pc\include\kobuki_bumper2pc):class Bumper2PcNodelet : public nodelet::Nodelet

Kobuki_nodelet.cpp (ork\src\kobuki-kinetic\kobuki_node\src\nodelet):class KobukiNodelet : public nodelet::Nodelet

Nodelet.cpp (ork\src\kobuki-kinetic\kobuki_auto_docking\src):class AutoDockingNodelet : public nodelet::Nodelet

Nodelet.cpp (ork\src\kobuki-kinetic\kobuki_controller_tutorial\src):class BumpBlinkControllerNodelet : public nodelet::Nodelet

Nodelet.cpp (ork\src\kobuki-kinetic\kobuki_random_walker\src):class RandomWalkerControllerNodelet : public nodelet::Nodelet

Nodelet.cpp (ork\src\kobuki-kinetic\kobuki_safety_controller\src):class SafetyControllerNodelet : public nodelet::Nodelet

3.1 Bumper2PcNodelet 

保险杆和悬崖传感器点云Nodelet节点实现

kobuki_bumper2pc.cpp详细说明

 pointcloud_pub_  = nh.advertise <sensor_msgs::PointCloud2> ("pointcloud", 10);

  core_sensor_sub_ = nh.subscribe("core_sensors", 10, &Bumper2PcNodelet::coreSensorCB, this);

Kobuki_ros::  sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);

 

保险杆和悬崖传感器点云Nodelet类实现

发布导航包可使用的点云话题

订阅Kobuki_ros发布的传感器消息，。

在导航中作为Nodelet与kobuki_node一起工作。

standalone.launch详细说明：

保险杆和悬崖传感器点云Nodelet启动文件

设置pointcloud_radius参数，缓冲器/悬崖传感器点云到底座半径范围

大概就是机器人半径加上costmap识别区用于应对机器人的惯性。

这个参数不容易设置:设置太小costmap会忽略点云，导致机器人撞上障碍。设置太大,障碍占据地方太大，围绕他们做导航，就可能失败。

启动nodelet_manager Nodelet管理器

重映射pointcloud，core_sensors两个话题

3.2 SafetyControllerNodelet代码关系

SafetyControllerNodelet->SafetyController

 

    time_to_extend_bump_cliff_events_ = ros::Duration(time_to_extend_bump_cliff_events);

    enable_controller_subscriber_ = nh_.subscribe("enable", 10, &SafetyController::enableCB,this);

    disable_controller_subscriber_ = nh_.subscribe("disable", 10, &SafetyController::disableCB,this);

    bumper_event_subscriber_ = nh_.subscribe("events/bumper", 10, &SafetyController::bumperEventCB,this);

    cliff_event_subscriber_  = nh_.subscribe("events/cliff",  10, &SafetyController::cliffEventCB,this);

    wheel_event_subscriber_  = nh_.subscribe("events/wheel_drop", 10, &SafetyController::wheelEventCB,this);

    reset_safety_states_subscriber_ = nh_.subscribe("reset", 10, &SafetyController::resetSafetyStatesCB,this);

    velocity_command_publisher_ = nh_.advertise< geometry_msgs::Twist >("cmd_vel", 10);

 

SafetyController订阅的消息由下node发布

Kobuki_ros：：

  version_info_publisher = nh.advertise < kobuki_msgs::VersionInfo > ("version_info",  100, true); // latched publisher

  controller_info_publisher = nh.advertise < kobuki_msgs::ControllerInfo > ("controller_info",  100, true); // latched publisher

  button_event_publisher = nh.advertise < kobuki_msgs::ButtonEvent > ("events/button", 100);

  bumper_event_publisher = nh.advertise < kobuki_msgs::BumperEvent > ("events/bumper", 100);

  cliff_event_publisher  = nh.advertise < kobuki_msgs::CliffEvent >  ("events/cliff",  100);

  wheel_event_publisher  = nh.advertise < kobuki_msgs::WheelDropEvent > ("events/wheel_drop", 100);

  power_event_publisher  = nh.advertise < kobuki_msgs::PowerSystemEvent > ("events/power_system", 100);

  input_event_publisher  = nh.advertise < kobuki_msgs::DigitalInputEvent > ("events/digital_input", 100);

  robot_event_publisher  = nh.advertise < kobuki_msgs::RobotStateEvent > ("events/robot_state", 100, true); // also latched

  sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);

  dock_ir_publisher = nh.advertise < kobuki_msgs::DockInfraRed > ("sensors/dock_ir", 100);

  imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data", 100);

  raw_imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data_raw", 100);

  raw_data_command_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_command", 100);

  raw_data_stream_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_stream", 100);

  raw_control_command_publisher = nh.advertise< std_msgs::Int16MultiArray > ("debug/raw_control_command", 100);

SafetyController::spin()

The SafetyController keeps track of bumper, cliff and wheel drop events. In case of the first two, Kobuki is commanded to move back. In the latter case, Kobuki is stopped. 

 

3.3 键盘控制

keyop_core.cpp

  keyinput_subscriber = nh.subscribe("teleop", 1, &KeyOpCore::remoteKeyInputReceived, this);

  velocity_publisher_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

  motor_power_publisher_ = nh.advertise<kobuki_msgs::MotorPower>("motor_power", 1);

 

 

3.4 话题关系

3.4.1 sensors/core

Auto_docking.launch.xml (kobuki_rapps\rapps\auto_docking):    <remap from="dock_drive/core" to="/mobile_base/sensors/core"/>

Bumper2pc.launch.xml (turtlebot_bringup\launch\includes\kobuki):    <remap from="bumper2pointcloud/core_sensors" to="mobile_base/sensors/core"/>

getDockIR.py (kobuki_auto_docking\scripts):    self.sub_core    = rospy.Subscriber("/mobile_base/sensors/core" , SensorState, self.SensorStateCallback)

Kobuki_ros.cpp (kobuki_node\src\library):  sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);

Mobile_base.launch.xml (turtlebot_bringup\launch\includes\create):    <remap from="turtlebot_node/sensor_state" to="mobile_base/sensors/core" />

Mobile_base.launch.xml (turtlebot_bringup\launch\includes\roomba):    <remap from="turtlebot_node/sensor_state" to="mobile_base/sensors/core" />

Test_analog_input.py (kobuki_testsuite\scripts):rospy.Subscriber("/mobile_base/sensors/core", SensorState, SensorStateCallback)

Test_battery_voltage.py (kobuki_testsuite\scripts):rospy.Subscriber("/mobile_base/sensors/core", SensorState, SensorStateCallback)

Test_input.py (kobuki_testsuite\scripts):  rospy.Subscriber("/mobile_base/sensors/core", SensorState, SensorStateCallback )

 

 

3.5 BumpBlinkControllerNodelet 

3.6 KobukiNodelet

Cmd_vel运行过程：

KobukiNodelet->KobukiRos（subscriber publisher）->Kobuki串口通信.

SafetyController订阅的消息由下node发布

Kobuki_ros：：

  version_info_publisher = nh.advertise < kobuki_msgs::VersionInfo > ("version_info",  100, true); // latched publisher

  controller_info_publisher = nh.advertise < kobuki_msgs::ControllerInfo > ("controller_info",  100, true); // latched publisher

  button_event_publisher = nh.advertise < kobuki_msgs::ButtonEvent > ("events/button", 100);

  bumper_event_publisher = nh.advertise < kobuki_msgs::BumperEvent > ("events/bumper", 100);

  cliff_event_publisher  = nh.advertise < kobuki_msgs::CliffEvent >  ("events/cliff",  100);

  wheel_event_publisher  = nh.advertise < kobuki_msgs::WheelDropEvent > ("events/wheel_drop", 100);

  power_event_publisher  = nh.advertise < kobuki_msgs::PowerSystemEvent > ("events/power_system", 100);

  input_event_publisher  = nh.advertise < kobuki_msgs::DigitalInputEvent > ("events/digital_input", 100);

  robot_event_publisher  = nh.advertise < kobuki_msgs::RobotStateEvent > ("events/robot_state", 100, true); // also latched

  sensor_state_publisher = nh.advertise < kobuki_msgs::SensorState > ("sensors/core", 100);

  dock_ir_publisher = nh.advertise < kobuki_msgs::DockInfraRed > ("sensors/dock_ir", 100);

  imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data", 100);

  raw_imu_data_publisher = nh.advertise < sensor_msgs::Imu > ("sensors/imu_data_raw", 100);

  raw_data_command_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_command", 100);

  raw_data_stream_publisher = nh.advertise< std_msgs::String > ("debug/raw_data_stream", 100);

  raw_control_command_publisher = nh.advertise< std_msgs::Int16MultiArray > ("debug/raw_control_command", 100);

 

void KobukiRos::subscribeTopics(ros::NodeHandle& nh)

{

  velocity_command_subscriber = nh.subscribe(std::string("commands/velocity"), 10, &KobukiRos::subscribeVelocityCommand, this);

  led1_command_subscriber =  nh.subscribe(std::string("commands/led1"), 10, &KobukiRos::subscribeLed1Command, this);

  led2_command_subscriber =  nh.subscribe(std::string("commands/led2"), 10, &KobukiRos::subscribeLed2Command, this);

  digital_output_command_subscriber =  nh.subscribe(std::string("commands/digital_output"), 10, &KobukiRos::subscribeDigitalOutputCommand, this);

  external_power_command_subscriber =  nh.subscribe(std::string("commands/external_power"), 10, &KobukiRos::subscribeExternalPowerCommand, this);

  sound_command_subscriber =  nh.subscribe(std::string("commands/sound"), 10, &KobukiRos::subscribeSoundCommand, this);

  reset_odometry_subscriber = nh.subscribe("commands/reset_odometry", 10, &KobukiRos::subscribeResetOdometry, this);

  motor_power_subscriber = nh.subscribe("commands/motor_power", 10, &KobukiRos::subscribeMotorPower, this);

  controller_info_command_subscriber =  nh.subscribe(std::string("commands/controller_info"), 10, &KobukiRos::subscribeControllerInfoCommand, this);

}

 

3.7 RandomWalkerControllerNodelet

RandomWalkerControllerNodelet->RandomWalkerController

  enable_controller_subscriber_ = nh_priv_.subscribe("enable", 10, &RandomWalkerController::enableCB, this);

    disable_controller_subscriber_ = nh_priv_.subscribe("disable", 10, &RandomWalkerController::disableCB, this);

    bumper_event_subscriber_ = nh_priv_.subscribe("events/bumper", 10, &RandomWalkerController::bumperEventCB, this);

    cliff_event_subscriber_ = nh_priv_.subscribe("events/cliff", 10, &RandomWalkerController::cliffEventCB, this);

    wheel_drop_event_subscriber_ = nh_priv_.subscribe("events/wheel_drop", 10,

                                                      &RandomWalkerController::wheelDropEventCB, this);

    cmd_vel_publisher_ = nh_priv_.advertise<geometry_msgs::Twist>("commands/velocity", 10);

    led1_publisher_ = nh_priv_.advertise<kobuki_msgs::Led>("commands/led1", 10);

    led2_publisher_ = nh_priv_.advertise<kobuki_msgs::Led> ("commands/led2", 10);

 

仔细阅读了SafetyControllerNodelet， 键盘控制，RandomWalkerControllerNodelet，KobukiNodelet相关的代码。

对kobuki框架下的控制流程了然于胸。

上述3种控制方式：安全控制，键盘控制，随机控制，通过发布下面两种topic进行控制。

commands/motor_power

commands/velocity

 

安全控制：通过订阅enable，disable，events/bumper，events/cliff，events/wheel_drop，reset，

在安全控制方式enable时，根据bumper，cliff的值，发布不同的cmd_vel消息。（cmd_vel和commands/velocity之间有映射关系，是同一个topic）

 

 

 

键盘控制:订阅了teleop，根据不同的键盘值发布cmd_vel，motor_power。

RandomWalkerController与上述两种大同小异。

 

 

KobukiNodelet会创建KobukiRos对象，

 

KobukiRos对象订阅了SafetyController，RandomWalkerController和keyop_core.cpp

发布的commands/motor_power，commands/velocity，commands/controller_info等等的命令topic,

 

 

KobukiRos对象发布了SafetyController，RandomWalkerController订阅的topic.

 

 

------------------------------

疑问点：

commands/controller_info（由KobukiRos对象发布）是和PID相关的topic，但是我搜索了整个工程没有发布commands/controller_info的node.仅KobukiRos对象发布了controller_info，或许这两个topic是同一个，

但是代码中没有commands/controller_info和controller_info之间的映射关系。