Kobuki控制插件
在上一节中介绍的Turtlebot的模型中有两个插件， 分别是底盘的控制插件kobuki_controller和深度摄像头的控制插件kinect_camera_controller。本文详细分析Kobuki控制插件的源码。
为了方便分析Turtlebot的相关源码，我们专门为之建立一个catkin的工作空间，并通过git把kobuki和kobuki_desktop的源码clone下来，编译之:
$ cd ~
$ mkdir -p turtlebot/src
$ cd turtlebot/src
$ git clone https://github.com/yujinrobot/kobuki_desktop.git
$ git clone https://github.com/yujinrobot/kobuki.git
$ cd ..
$ catkin_make
$ source devel/setup.bash
切换到我们要考察的kobuki控制插件的包kobuki_gazebo_plugin的目录下，查看其目录结构一共只有3个目录7个文件。其中CHANGELOG.txt是更新日志，与实现无关。
CMakeLists.txt为项目的CMake编译指导文件。如创建和编译Package一文中所描述的，package.xml文件描述了包的各种依赖信息。
我们需要关注的源文件只有一个*.h和三个*.cpp文件。
$ roscd kobuki_gazebo_plugin
$ tree .
.
├── CHANGELOG.rst
├── CMakeLists.txt
├── include
│ └── kobuki_gazebo_plugins
│ └── gazebo_ros_kobuki.h
├── package.xml
└── src
├── gazebo_ros_kobuki.cpp
├── gazebo_ros_kobuki_loads.cpp
└── gazebo_ros_kobuki_updates.cpp
3 directories, 7 files
1. CMakelists.txt
首先声明了CMake版本以及项目名称
cmake_minimum_required(VERSION 2.8.3)
project(kobuki_gazebo_plugins)
接着通过find_package查找各个相关的库模块，其中gazebo和catkin是必须的两个包。
find_package(gazebo REQUIRED)
find_package(catkin REQUIRED COMPONENTS gazebo_ros gazebo_plugins geometry_msgs kobuki_msgs
nav_msgs roscpp sensor_msgs std_msgs tf)
然后使用catkin_pacakge设定ROS的catkin编译工具
catkin_package(INCLUDE_DIRS include
LIBRARIES gazebo_ros_kobuki
CATKIN_DEPENDS gazebo_ros gazebo_plugins geometry_msgs kobuki_msgs nav_msgs roscpp sensor_msgs std_msgs tf)
开启C++11编译
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
通过link_directories添加Gazebo相关的动态链接库目录，
通过include_directories添加头文件目录。
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
下面是本文的主角，通过add_library指引构造链接库libgazebo_ros_kobuki.so，它依赖于三个*.cpp文件。
此外还用add_dependencies添加依赖关系。
add_library(gazebo_ros_kobuki src/gazebo_ros_kobuki.cpp
src/gazebo_ros_kobuki_updates.cpp
src/gazebo_ros_kobuki_loads.cpp)
add_dependencies(gazebo_ros_kobuki ${catkin_EXPORTED_TARGETS})
指示编译目标
target_link_libraries(gazebo_ros_kobuki ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
添加安装规则，指引安装目录
install(TARGETS gazebo_ros_kobuki DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
2. 插件的初始化操作
2.1 GazeboRosKobuki构造函数
在Gazebo的仿真插件一文中，我们已经介绍过Gazebo的Model类型插件都是继承自ModelPlugin。 我们这里的Kobuki控制插件GazeboRosKobuki也是一个继承自ModelPlugin的插件，下面左边是它的构造函数，在构造函数中对防跌落传感器和轮子速度指令赋予了初值。 shutdown_requested_是一个bool类型的变量用于控制关闭插件。在右边的析构函数中，只是将shutdown_requested_置位，标识将要关闭插件，其它资源释放操作都被注释了。
|
|
2.2 Load函数
Gazebo在装载插件的时候，会调用一个Load的成员函数。它有两个参数，其中parent是一个指向与本插件相关联的模型的指针，在Load函数的一开始就将之赋予了成员变量model_。 另一个参数sdf则是一个指向本插件SDF元素的指针，我们可以通过该指针借助Gazebo的API解析SDF格式下描述的机器人模型，在第9行中就用一个成员变量sdf_保存之。
在第8行中通过这两个参数构建了一个GazeboRos类型的对象。 这是一个在gazebo_plugins的头文件gazebo_ros_utils.h中定义的类, 它是Gazebo Ros的一个辅助类，声明了各种参数和rosnode的句柄。
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_) {
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
gazebo_ros_ = GazeboRosPtr(new GazeboRos(model_, sdf, "Kobuki"));
sdf_ = sdf;
接下来，确认一下ROS的环境是否已经开启了。
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized()) {
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
下面，通过SDF文件指针sdf获取机器人的名称。在介绍Turtlebot描述模型的时候， 提到过这个控制插件没有指定相关的标签，意味着它是对机器人模型的描述。所以其父标签就是机器人的标签，通过GetPrent()可以访问到整个机器人的模型描述，进而通过Get("name")获得机器人的名称。 并将之用作ROS节点的名称。最后在第20行，通过模型指针parent获知仿真世界。
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->Get("name");
gzdbg << "Plugin model name: " << model_name << "\n";
node_name_ = model_name;
world_ = parent->GetWorld();
接下来，通过调用在gazebo_ros_kobuki_loads.cpp中实现的各种准备函数完成对系统的初始化操作，包括准备电机控制、准备发布里程计数据、准备各种传感器的初值、
准备接收速度指令等。最后通过setupRosApi完成ROS系统接口的初始化操作。
prepareMotorPower();
preparePublishTf();
if(prepareJointState() == false)
return;
if(prepareWheelAndTorque() == false)
return;
prepareOdom();
if(prepareVelocityCommand() == false)
return;
if(prepareCliffSensor() == false)
return;
if(prepareBumper() == false)
return;
if(prepareIMU() == false)
return;
setupRosApi(model_name);
最后，通过仿真世界获取原先的仿真更新时间。这些操作完成之后，打印初始化结束日志。并通过updateConnection监听Gazebo的更新事件，
Gazebo在每一个仿真迭代之后广播一次该事件。在每一次更新事件发生后，都会调用一次OnUpdate的成员函数来执行用户在插件中定义的各种更新操作。
#if GAZEBO_MAJOR_VERSION >= 9
prev_update_time_ = world_->SimTime();
#else
prev_update_time_ = world_->GetSimTime();
#endif
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateEnd(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
我们知道在Load函数中调用了在gazebo_ros_kobuki_loads.cpp中的一系列prepare函数，其内容无非是完成各种初始化操作，就不再细说了。这里需要关注讲一下setupRosApi函数， 在这个函数中注册发布和订阅了若干个主题，下面以里程计数据和速度控制指令为例分别介绍如何发布和订阅主题的。
下面是发布里程计主题的过程。首先用一个string类型的变量指定需要发布的主题名称，这里是"odom"。然后通过在Load函数中第8行构建的GazeboRos对象注册需要发布的消息， 最后打印一个日志。
// odom
std::string odom_topic = "odom";
odom_pub_ = gazebo_ros_->node()->advertise(odom_topic, 1);
ROS_INFO("%s: Advertise Odometry[%s]!", gazebo_ros_->info(), odom_topic.c_str());
下面是订阅速度指令主题的过程。类似的，用一个string类型变量指定主题名称。在通过ROS的订阅过程中，指定了4个参数，分别是主题名称、消息缓存队列大小、回调函数、和参数。 需要特别关注的是最后一个参数this。虽然在cmdVelCB的参数列表中，只有一个geometry_msgs::TwistConstPtr类型的参数。但在编译过程中，C++的编译器还是会为这个函数添加一个参数， 是一个指向对象本身的指针，也就是this。为了能够正确的调用cmdVelCB函数，就需要遵循C++的这一约定，以this作为第0个参数调用。
// cmd_vel
std::string cmd_vel_topic = base_prefix + "/commands/velocity";
cmd_vel_sub_ = gazebo_ros_->node()->subscribe(cmd_vel_topic, 100, &GazeboRosKobuki::cmdVelCB, this);
ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), cmd_vel_topic.c_str());
在setupRosApi中一共订阅了三个主题，分别注册了回调函数motorPowerCB、cmdVelCB和resetOdomCB。
3. 更新操作
我们已经知道，在每一个仿真周期中，都会产生一个更新事件，并调用插件对象的成员函数OnUpdate，这点由Load函数中最后的一条语句决定的。在OnUpdate函数的一开始， 通过ros的spinOnce操作处理ROS的回调请求。
void GazeboRosKobuki::OnUpdate()
{
/* First process ROS callbacks */
ros::spinOnce();
接下来更新时间和时间增量。
/* Update current time and time step */
common::Time time_now;
#if GAZEBO_MAJOR_VERSION >= 9
time_now = world_->SimTime();
#else
time_now = world_->GetSimTime();
#endif
common::Time step_time = time_now - prev_update_time_;
prev_update_time_ = time_now;
最后调用在gazebo_ros_kobuki_updates.cpp中实现的各种更新函数。这些更新函数就是计算各个传感器的数据，然后把新的数据发布出去。
updateJointState();
updateOdometry(step_time);
updateIMU();
propagateVelocityCommands();
updateCliffSensor();
updateBumper();
}
4. 访问Gazebo中传感器的数据
我们在介绍Turtlebot的描述模型的时候，就已经提到过在底盘Kobuki上装有三个防跌落的传感器、一个碰撞传感器和一个IMU。 它们使用的分别是Gazebo原生的ray、contact和imu类型的传感器。那么该如何获取这些传感器的数据呢?我们以碰撞传感器bumper为例解释。
首先让我们再看一下kobuki_gazebo.urdf.xacro文件中引入Gazebo插件的xml描述，其中的各个子标签都是对Kobuki控制插件的各种配置。我们关注第14行的bumper_name标签，其值为"bumpers"。
<gazebo>
<plugin name="kobuki_controller" filename="libgazebo_ros_kobuki.so">
<publish_tf>1</publish_tf>
<left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
<right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
<wheel_separation>.230</wheel_separation>
<wheel_diameter>0.070</wheel_diameter>
<torque>1.0</torque>
<velocity_command_timeout>0.6</velocity_command_timeout>
<cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
<cliff_sensor_center_name>cliff_sensor_front</cliff_sensor_center_name>
<cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
<cliff_detection_threshold>0.04</cliff_detection_threshold>
<bumper_name>bumpers</bumper_name>
<imu_name>imu</imu_name>
</plugin>
</gazebo>
下面是添加bumper传感器的xml片段，可以看到其中的sensor子标签有两个属性，type是传感器的类型取值为"contact"，name就是传感器的名称，正是上面kobuki_controller的bumper_name子标签的取值。
<gazebo reference="base_link">
<mu1>0.3</mu1>
<mu2>0.3</mu2>
<sensor type="contact" name="bumpers">
<always_on>1</always_on>
<update_rate>50.0</update_rate>
<visualize>true</visualize>
<contact>
<collision>base_footprint_fixed_joint_lump__base_collision</collision>
</contact>
</sensor>
</gazebo>
在bumper传感器的初始化过程中，首先通过SDF指针查找bumper_name子标签，并据此获得碰撞传感器的名称。
bool GazeboRosKobuki::prepareBumper()
{
std::string bumper_name;
if (sdf_->HasElement("bumper_name"))
{
bumper_name = sdf_->GetElement("bumper_name")->Get();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return false;
}
然后，通过gazebo的应用接口SensorManager，根据刚查找到的传感器名称获得传感器对象， 并用一个ContactSensor类型的指针bumper_记录之。
bumper_ = std::dynamic_pointer_cast(sensors::SensorManager::Instance()->GetSensor(bumper_name));
最后判定是否成功获得传感器对象，并设定开启传感器。
if (!bumper_)
{
ROS_ERROR_STREAM("Couldn't find the bumpers in the model! [" << node_name_ <<"]");
return false;
}
bumper_->SetActive(true);
return true;
}
完成初始化工作，得到指向传感器对象的指针之后，我们就可以通过传感器的成员函数获取传感器的数据了。比如说我们这里的bumper_ 有一个Contacts的函数， 它将返回一个包含所有与传感器的碰撞信息的消息，数据类型是msgs::Contacts。在bumper传感器的更新函数中，先通过这个函数获取碰撞信息。
void GazeboRosKobuki::updateBumper()
{
// reset flags
bumper_left_is_pressed_ = false;
bumper_center_is_pressed_ = false;
bumper_right_is_pressed_ = false;
// parse contacts
msgs::Contacts contacts;
contacts = bumper_->Contacts();
接着通过模型对象model_获取机器人在世界中的位姿。
double robot_heading;
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Pose3d current_pose = model_->WorldPose();
robot_heading = current_pose.Rot().Yaw();
#else
math::Pose current_pose = model_->GetWorldPose();
robot_heading = current_pose.rot.GetYaw();
#endif
然后遍历所有的碰撞信息，计算碰撞在机器人上的高度。
for (int i = 0; i < contacts.contact_size(); ++i)
{
double rel_contact_pos;
#if GAZEBO_MAJOR_VERSION >= 9
rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.Pos().Z();
#else
rel_contact_pos = contacts.contact(i).position(0).z() - current_pose.pos.z;
#endif
由于实际的碰撞传感器只是装在机器人上的几个簧片和轻触开关而已，必须在一定的高度范围内才能判定为触发了碰撞传感器。接着计算碰撞发生的实际角度，
30°到90°的范围内认为触发了左侧的碰撞簧片，-30°到30°认为是中间的碰撞簧片，-90°到-30°则是右侧的簧片。
if ((rel_contact_pos >= 0.01) && (rel_contact_pos <= 0.13))
{
// 计算碰撞发生的实际角度
double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x());
double relative_contact_angle = global_contact_angle - robot_heading;
if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6)))
bumper_left_is_pressed_ = true;
else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6)))
bumper_center_is_pressed_ = true;
else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2)))
bumper_right_is_pressed_ = true;
}
}
接下来就是更新碰撞状态，发布碰撞事件消息。
if (bumper_left_is_pressed_ && !bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_left_is_pressed_ && bumper_left_was_pressed_)
{
bumper_left_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::LEFT;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_center_is_pressed_ && !bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_center_is_pressed_ && bumper_center_was_pressed_)
{
bumper_center_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::CENTER;
bumper_event_pub_.publish(bumper_event_);
}
if (bumper_right_is_pressed_ && !bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = true;
bumper_event_.state = kobuki_msgs::BumperEvent::PRESSED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
else if (!bumper_right_is_pressed_ && bumper_right_was_pressed_)
{
bumper_right_was_pressed_ = false;
bumper_event_.state = kobuki_msgs::BumperEvent::RELEASED;
bumper_event_.bumper = kobuki_msgs::BumperEvent::RIGHT;
bumper_event_pub_.publish(bumper_event_);
}
}
其它类型传感器的数据也可以通过类似的形式获得，我们可以参考Gazebo的API介绍获取更为详细的说明。
5. 总结
Koboki的控制插件源码只有3个*.cpp文件和一个*.h文件。总体上可以分为两个部分：插件的初始化操作和仿真更新操作。
它订阅了"${base_prefix}/commands/motor_power", "${base_prefix}/commands/reset_odometry"和"${base_prefix}/commands/velocity"三个主题，分别用于控制电机、重置里程计、控制底盘运动速度。
此外还发布了"joint_states", "odom", "${base_prefix}/events/cliff", "${base_prefix}/events/bumper", "${base_prefix}/events/imu_data"等传感器数据相关的主题。 在ROS环境下通过订阅这些主题就可以获得底盘的状态。