www.gusucode.com > robotexamples 工具箱 matlab源码程序 > robotexamples/ros/TurtleBotBasicExplorationExample.m
%% Explore Basic Behavior of the TurtleBot
%% Introduction
% This example helps you to explore
% basic autonomy with the TurtleBot(R). The described behavior drives the robot
% forward and changes its direction when there is an obstacle.
% You will subscribe to the laser scan topic and publish the velocity
% topic to control the TurtleBot.
%
% Prerequisites: <docid:robotics_examples.example-TurtleBotCommunicationExample>
% Copyright 2014-2016 The MathWorks, Inc.
%% Hardware Support Package for TurtleBot
% This example gives an overview of working with a TurtleBot using its native ROS interface.
% The Robotics System Toolbox(TM) Support Package for TurtleBot(R)-Based Robots
% provides a more streamlined interface to TurtleBot. It allows you to:
%
% * Acquire sensor data and send control commands without explicitly calling ROS commands
% * Communicate transparently with a simulated robot in Gazebo or with a physical TurtleBot
%
% To install the support package, open *Add-Ons > Get Hardware Support
% Packages* on the MATLAB *Home* tab and select "TurtleBot-Based Robots".
% Alternatively, use the |<docid:robotics_ref.bu5jj8h roboticsAddons>| command.
%
%% Connect to the TurtleBot
% Make sure you have a TurtleBot running either in simulation through
% Gazebo(R) or on real hardware. Refer to
% <docid:robotics_examples.example-GettingStartedWithGazeboExample> or
% <docid:robotics_examples.example-GettingStartedWithRealTurtleBotExample> for the startup
% procedure. Any Gazebo world works while running in simulation, however,
% |Gazebo TurtleBot World| is the most interesting for the purposes of this
% example.
%
% * Initialize ROS. Connect to the TurtleBot by replacing
% the sample IP address (192.168.1.1) with the IP address of the TurtleBot.
%
% ipaddress = '192.168.1.1'
%
%%
rosinit(ipaddress)
%%
% * Create a publisher for the robot's velocity and create a message for that topic.
%%
robot = rospublisher('/mobile_base/commands/velocity');
velmsg = rosmessage(robot);
%% Receive Scan Data
% Make sure that you start the
% Kinect(R) camera if you are working with real TurtleBot hardware. That
% command is: |roslaunch turtlebot_bringup 3dsensor.launch|.
% You must execute the command in a terminal on the TurtleBot.
% The TurtleBot uses the Kinect data to simulate a laser scan that is
% published on the |/scan| topic. For the remainder of this example, the
% term "laser scan" refers to data published on this topic.
%
% * Subscribe to the topic |/scan|.
%%
laser = rossubscriber('/scan');
%%
% * Wait for one laser scan message to arrive and then display it.
%%
scan = receive(laser,3)
figure
plot(scan);
%%
% If you see an error, it is possible that the laser scan topic is
% not receiving any data. If you are running in simulation, try restarting Gazebo.
% If you are using hardware, make sure the that you started the Kinect
% camera properly.
%
% * In Gazebo, the scan is similar to this scan:
%
% <<laser_scan_explore.png>>
%
% * Run the following lines of code, which plot a live laser scan feed for
% twenty seconds. Move an object in front of the TurtleBot and bring it
% close enough until it no longer shows up in the plot window.
% The laser scan has a limited range because of hardware
% limitations of the Kinect camera. The Kinect has a minimum sensing range of
% 0.8 meters and a maximum range of 4 meters. Any objects outside these
% limits will not be detected by the sensor.
%%
tic;
while toc < 10
scan = receive(laser,3);
plot(scan);
end
%% Simple Obstacle Avoidance
% Based on the distance readings from the laser scan, you can implement a
% simple obstacle avoidance algorithm. You can use a simple |while| loop to
% implement this behavior.
%
% * Set some parameters that will be used in the processing loop. You can modify
% these values for different behavior.
%%
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
%%
% * Run a loop to move the robot forward and compute the closest
% obstacles to the robot. When an obstacle is within the limits of the
% |distanceThreshold|, the robot turns. This loop stops after 20
% seconds of run time. CTRL+C (or Control+C on the Mac) also stops this loop.
%%
tic;
while toc < 20
% Collect information from laser scan
scan = receive(laser);
plot(scan);
data = readCartesian(scan);
x = data(:,1);
y = data(:,2);
% Compute distance of the closest obstacle
dist = sqrt(x.^2 + y.^2);
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 = 0;
end
send(robot,velmsg);
end
%% Disconnect from the Robot
% * It is good practice to clear the workspace of publishers,
% subscribers, and other ROS related objects when you are finished with
% them.
%
% clear
%%
% * It is recommended to use |rosshutdown| once you are done working with
% the ROS network. Shut down the global node and disconnect from the
% TurtleBot.
%%
rosshutdown
%% More Information
% The laser scan has a minimum range at which it no
% longer sees objects in its way. That minimum is somewhere around 0.5
% meters from the Kinect camera.
%
% The laser scan cannot detect glass walls. Following is an
% image from the Kinect camera:
%
% <<turtlebot_view.png>>
%
% Here is the corresponding laser scan:
%
% <<turtlebot_scan.png>>
%
% The trash can is visible, but you cannot see the glass wall.
% When you use the TurtleBot in areas with windows or
% walls that the TurtleBot might not be able to detect, be aware of the limitations
% of the laser scan.
%% Next Steps
%
% * Refer to the next example: <docid:robotics_examples.example-TurtleBotTeleoperationExample>
%%
displayEndOfDemoMessage(mfilename)