Explore Basic Behavior of the TurtleBot ---3

原创博文：转载请标明出处（周学伟）：http://www.cnblogs.com/zxouxuewei/tag/

Introduction

此示例帮助您使用turtlebot的自主性。驱动机器人向前移动并且当存在障碍物时改变其方向。您将订阅激光扫描主题并发布速度主题以控制turtlebot。

Hardware Support Package for turtlebot

本示例使用ROS接口的TurtleBot。基于TurtleBot的机器人支持包为TurtleBot提供了一个更简洁的界面。

它允许您：
获取传感器数据并发送控制命令，而不显式调用ROS命令。
与Gazebo中的模拟机器人或物理TurtleBot进行透明通信。

要安装支持包，在MATLAB Home选项卡上打开Add-Ons>获取硬件支持包，然后选择“TurtleBot-Based Robots”。或者，使用roboticsAddons命令。

Connect to the TurtleBot

确保你有一个TurtleBot运行在通过Gazebo或真实硬件的模拟。有关启动过程，请参阅开始使用Gazebo和模拟TurtleBot或开始使用Real TurtleBot。

　　再此我没有Turtlebot 也不想用Gazebo，我用自己开发的zxbot机器人。按照相应的指令启动机器人。

链接机器人的wifi ZXBOT，远程登录机器人：

ssh ubuntu@192.168.1.159
roslaunch odom_tf_package zxbot_start.launch

keysi_start.launch文件内容如下：github链接：https://github.com/ZXWBOT

<launch>

    <param name="use_sim_time" value="false" />

    <!--Set the TF tree for the keysi robot-->

    <node name="link_laser"     pkg="tf" type="static_transform_publisher" args="0.15 0 0.15 0 0 1.0 6.12323399574e-17 base_link laser 50"/>

    <node name="link_camera"    pkg="tf" type="static_transform_publisher" args="0.15 0 0.15 0 0 0 base_link camera_link 50"/>

    <node name="link_footprint" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 base_link base_footprint 50"/>

    <!--Start the robot's base control code and publish the odometer data-->

    <node pkg="keysi_package" type="keysi_start_node" name="publish_odom" output="screen">

      <param name="usart_port"         type="string" value="/dev/zxbot_base"/>

        <param name="baud_data"          type="int"    value=""/>

      <param name="robot_frame_id"     type="string" value="base_link"/>

      <param name="smoother_cmd_vel"   type="string" value="/smoother_cmd_vel"/>

    </node>

    <!--Open the Radar Startup Package and publish the Scan data-->

    <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">

      <param name="serial_port"         type="string" value="/dev/zxbot_rplidar"/>

      <param name="serial_baudrate"     type="int"    value=""/>

      <param name="frame_id"            type="string" value="laser"/>

      <param name="inverted"            type="bool"   value="false"/>

      <param name="angle_compensate"    type="bool"   value="true"/>

      <param name="Angle_Laser"         type="int"    value=""/>

   </node>

      <arg name="node_name"             value="velocity_smoother"/>

      <arg name="nodelet_manager_name"  value="nodelet_manager"/>

      <arg name="config_file"           value="$(find keysi_package)/config/yocs_velocity_smoother.yaml"/>

      <arg name="raw_cmd_vel_topic"     value="cmd_vel"/>

      <arg name="smooth_cmd_vel_topic"  value="smoother_cmd_vel"/>

      <arg name="robot_cmd_vel_topic"   value="robot_cmd_vel"/>

      <arg name="odom_topic"            value="odom"/>

    <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>

    <include file="$(find yocs_velocity_smoother)/launch/velocity_smoother.launch">

      <arg name="node_name"             value="$(arg node_name)"/>

      <arg name="nodelet_manager_name"  value="$(arg nodelet_manager_name)"/>

      <arg name="config_file"           value="$(arg config_file)"/>

      <arg name="raw_cmd_vel_topic"     value="$(arg raw_cmd_vel_topic)"/>

      <arg name="smooth_cmd_vel_topic"  value="$(arg smooth_cmd_vel_topic)"/>

      <arg name="robot_cmd_vel_topic"   value="$(arg robot_cmd_vel_topic)"/>

      <arg name="odom_topic"            value="$(arg odom_topic)"/>

    </include>

</launch>

初始化ROS。通过用ZXBOT的IP地址替换示例IP地址（192.168.1.1），连接到ZXBOT。

ipaddress = '192.168.1.215'   //机器人主机的IP

rosinit(ipaddress)

为机器人的速度创建一个发布者，并为该主题创建一条消息。

如果你使用的是ZXBOT：
　　robot = rospublisher('/cmd_vel');

   velmsg = rosmessage(robot); 
如果你使用的是TURTLEBOT
　　robot = rospublisher('/mobile_base/commands/velocity'); 
   velmsg = rosmessage(robot);

Receive Scan Data

确保你的硬件已经发布了/scan数据。Subscribe to the topic /scan.

//在机器人端
Turtlebot roslaunch turtlebot_bringup 3dsensor.launch
zxbot roslaunch odom_tf_package zxbot_start.launch

//MATLAB
laser = rossubscriber('/scan');

scan = receive(laser,3)

figure

plot(scan);

如果您看到错误，可能是激光扫描主题没有接收任何数据。如果您正在模拟中运行，请尝试重新启动Gazebo。如果您使用硬件，请确保您已正确启动Kinect相机或者激光雷达。

运行以下代码行，其中绘制实时激光扫描进给二十秒。移动TurtleBot或zxbot前面的一个对象，并使它足够近，直到它不再显示在绘图窗口中。

  tic;

  while toc <

      scan = receive(laser,);

      plot(scan);

  end

Simple Obstacle Avoidance

基于从激光扫描的距离读数，您可以实现一个简单的障碍物回避算法。你可以使用一个简单的while循环来实现这个行为。

设置将在处理循环中使用的一些参数。您可以根据不同的行为修改这些值。

  spinVelocity = 0.6;       % Angular velocity (rad/s)

  forwardVelocity = 0.1;    % Linear velocity (m/s)

  backwardVelocity = -0.02; % Linear velocity (reverse) (m/s)

  distanceThreshold = 0.6;  % Distance threshold (m) for turning

运行循环以向前移动机器人并计算与机器人最近的障碍物。当障碍物在distanceThreshold的界限内时，机器人转动。该循环在运行时间20秒后停止。 CTRL + C（或Mac上的Control + C）也会停止此循环

tic;

  while toc <

      % Collect information from laser scan

      scan = receive(laser);

      plot(scan);

      data = readCartesian(scan);

      x = data(:,);

      y = data(:,);

      % Compute distance of the closest obstacle

      dist = sqrt(x.^ + y.^);

      minDist = min(dist);

      % Command robot action

      if minDist < distanceThreshold

          % If close to obstacle, back up slightly and spin

          velmsg.Angular.Z = spinVelocity;

          velmsg.Linear.X = backwardVelocity;

      else

          % Continue on forward path

          velmsg.Linear.X = forwardVelocity;

          velmsg.Angular.Z = ;

      end

      send(robot,velmsg);

  end

下面你就会看到你的机器人开始移动并且避开障碍物。

Disconnect from the Robot

最好在完成发布者，订阅者和其他ROS相关对象的工作区后清除它们。

clear

一旦你完成使用ROS网络，建议使用rosshutdown。关闭全局节点并从TurtleBot或者zxbot断开连接。

 rosshutdown