在ROS中开始自主机器人仿真 - 5 机器人环境探索与避障

基于Turtlebot的自主移动机器人仿真. 本文中所述的自主移动机器人采用turtlebot, 从Kinect 采集点云传入obstacle avoidance 节点中,给出避障转向; navigation节点根据机器人避障转向信息和收到的指令信息做出最终的控制决策,传入到locomotion节点; 最后locomotion节点给出机器人的运动控制命令.

/**
 * @copyright  Manfredas Zabarauskas 2013. All rights reserved. This project is released under
 *             CC BY-NC-SA （Creative Commons Attribution-NonCommercial-ShareAlike） license.
 */
#include 

// STL includes
#include 

// Kobuki includes
#include 

// RPLocomotion includes
#include 
#include 

int main（int argc char** argv）
{
    // Initialize ROS
    ros::init（argc argv “turtlebotslam_locomotion“）;

    // Get the handle to the ROS node
    ros::NodeHandle node;

    // Start the worker node
    RPLocomotionNode worker_node（node）;
}


RPLocomotionNode::RPLocomotionNode（ros::NodeHandle& node） :
    node（node）
    processing_bumper_event（false）
    locomotion_message（new geometry_msgs::Twist（））
    FRAMING_LINEAR_VELOCITY（FRAMING_LINEAR_VELOCITY_DEFAULT）
    FRAMING_ANGULAR_VELOCITY（FRAMING_ANGULAR_VELOCITY_DEFAULT）
    OBSTACLE_AVOIDANCE_LINEAR_VELOCITY（OBSTACLE_AVOIDANCE_LINEAR_VELOCITY_DEFAULT）
    OBSTACLE_AVOIDANCE_ANGULAR_VELOCITY（OBSTACLE_AVOIDANCE_ANGULAR_VELOCITY_DEFAULT）
{
    // Initialize publishers/subscribers/timers
    locomotion_publisher = node.advertise（“/turtlebotslam/locomotion/velocity“ 1）;
    state_publisher = node.advertise（“/turtlebotslam/locomotion/state“ 1）;
    motor_power_publisher = node.advertise（“/turtlebotslam/locomotion/motor_power“ 1）;

    navigation_subscriber = node.subscribe（“/turtlebotslam/navigation/driving_direction_and_source“ 1 &RPLocomotionNode::navigationMessageCallback this）;
    bumper_subscriber = node.subscribe（“/mobile_base/events/bumper“ 1 &RPLocomotionNode::bumperEventCallback this）;

    double approximate_sleep_time = 2.0 * M_PI / OBSTACLE_AVOIDANCE_ANGULAR_VELOCITY;
    bumper_timer = node.createWallTimer（ros::WallDuration（approximate_sleep_time） &RPLocomotionNode::bumperTimerCallback this true false）;

    setProcessingBumperEvent（false）;
    clearLocomotionMessage（）;

    motorsEnabled（true）;

    // Spin at 5 Hz
    ros::Rate rate（5）;
    while （ros::ok（））
    {

        locomotion_message_mutex.lock（）;
        {
            locomotion_publisher.publish（locomotion_message）;
        }
        locomotion_message_mutex.unlock（）;

        ros::spinOnce（）;
        rate.sleep（）;
    }
};


void RPLocomotionNode::getOverridableParameters（）
{
    node.getParamCached（“/turtlebotslam/locomotion_node/framing_linear_velocity“ FRAMING_LINEAR_VELOCITY）;
    node.getParamCached（“/turtlebotslam/locomotion_node/framing_angular_velocity“ FRAMING_ANGULAR_VELOCITY）;
    node.getParamCached（“/turtlebotslam/locomotion_node/obstacle_avoidance_linear_velocity“ OBSTACLE_AVOIDANCE_LINEAR_VELOCITY）;
    node.getParamCached（“/turtlebotslam/locomotion_node/obstacle_avoidance_angular_velocity“ OBSTACLE_AVOIDANCE_ANGULAR_VELOCITY）;
}


void RPLocomotionNode::setProcessingBumperEvent（bool processing）
{
    processing_bumper_event

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-04-05 12:37  turtlebotslam_gazebo\
     文件        2147  2017-04-05 12:16  turtlebotslam_gazebo\package.xml
     文件        6843  2017-04-05 12:16  turtlebotslam_gazebo\CMakeLists.txt
     目录           0  2017-04-05 12:31  turtlebotslam_gazebo\rviz\
     文件       11555  2018-01-31 09:58  turtlebotslam_gazebo\rviz\navigation.rviz
     目录           0  2018-02-22 08:08  turtlebotslam_gazebo\worlds\
     文件        8540  2018-02-22 08:07  turtlebotslam_gazebo\worlds\cafe.world
     文件       58091  2017-04-11 14:07  turtlebotslam_gazebo\worlds\barrier.world
     文件       53024  2017-04-10 09:32  turtlebotslam_gazebo\worlds\playground.world
     文件         669  2016-09-27 06:39  turtlebotslam_gazebo\worlds\empty.world
     文件       15798  2015-09-16 02:57  turtlebotslam_gazebo\worlds\corridor.world
     目录           0  2018-01-31 09:48  turtlebotslam_gazebo\launch\
     文件         730  2018-01-31 09:49  turtlebotslam_gazebo\launch\gmapping_demo.launch
     文件         666  2016-11-09 07:58  turtlebotslam_gazebo\launch\keyop.launch
     文件         146  2017-04-05 13:21  turtlebotslam_gazebo\launch\turtlebotslam_rviz.launch
     文件        2452  2018-02-23 08:04  turtlebotslam_gazebo\launch\turtlebotslam_gazebo.launch
     文件         486  2017-04-12 10:15  turtlebotslam_gazebo\launch\keyboard_teleop.launch
     目录           0  2017-04-05 12:16  turtlebotslam_gazebo\src\
     目录           0  2017-04-11 08:55  turtlebotslam_gazebo\launch\includes\
     文件        1217  2017-04-11 10:56  turtlebotslam_gazebo\launch\includes\kobuki.launch.xml
     文件        1431  2015-09-16 02:57  turtlebotslam_gazebo\launch\includes\create.launch.xml
     文件        1431  2015-09-16 02:57  turtlebotslam_gazebo\launch\includes\roomba.launch.xml
     目录           0  2017-04-11 06:38  turtlebotslam_locomotion\
     文件        6908  2017-04-11 06:38  turtlebotslam_locomotion\CMakeLists.txt
     文件        2257  2017-04-11 06:33  turtlebotslam_locomotion\package.xml
     目录           0  2013-09-18 09:12  turtlebotslam_locomotion\param\
     文件         151  2013-09-18 09:12  turtlebotslam_locomotion\param\keyop_smoother.yaml
     目录           0  2013-09-18 09:12  turtlebotslam_locomotion\launch\
     文件        2039  2017-04-11 13:42  turtlebotslam_locomotion\launch\locomotion.launch
     目录           0  2017-04-11 06:35  turtlebotslam_locomotion\src\
     文件        8028  2017-04-13 01:30  turtlebotslam_locomotion\src\locomotion.cpp
............此处省略28个文件信息