Page tree
Skip to end of metadata
Go to start of metadata

References and Prerequisites

Git Repository / ROS Package

As in the last couple assignments, your submission for this assignment will be a Git repository that contains a ROS package.  The name of the repository on the gitlab.nps.edu server should be mrc_hw4 and it should be a ROS package (catkin_crate_pkg).  You might look at the last assignment to refresh your memory on this setup.

Exercise 1: MATLAB Turtle Letters

In the previous assignment you used used the rostopic and rosservice Linux commands to draw the first two letters of your name.  In this exercise your job is to port (translate) that script to MATLAB.

Step 1: Launch turtle simulator from command line

Create a launch file in your package called launch/turtle.launch.  The launch script should start the ROS master and the turtlesim_node.  Open a new terminal and start the simulator like this...

roslaunch mrc_hw4 turtle.launch

Step 2: MATLAB commanding of turtle sim

In the MATLAB editor create a file in your ROS package called /matlab/turtleletterstwo.m
This script should use rossrvclient and rospublisher MATLAB functionality (as described in http://www.mathworks.com/help/robotics/examples/exchange-data-with-ros-publishers-and-subscribers.html) to generate the first two letters of your name as we did in the last assignment.  There are also examples in the lecture slides from this week (see the MRC Schedule - Summer 2016) which you may use to get started.

Add both the turtle.launch and turtleletterstwo.m files to your repository; commit and push.

Exercise 2: MATLAB Turtle Feedback Control

The turtlesim_node is a good first example of a ROS-enabled robot.  It receives commands on the /turtle1/cmd_vel topic and reports is position on the /turtle1/pose topic.

Create a file matlab/turtle_waypoint.m that defines the MATLAB function dist=turtle_waypoint(X,Y).  This function should use feedback to drive your turtle to within 0.1 units of the provided goal waypoint (X,Y).  Your function should initialize the ROS communications at the beginning (rosinit) and shutdown communications at the end (rosshutdown).  You will want to subscribe the the pose updates and publish velocity commands to move the turtle toward the goal.  (Hint: the atan2() function is handy for calculating the angle between the goal and the turtle's current location).  When the turtle gets within 0.1 units of the goal, the function should exit, returning the distance to the goal.

For this first controller, it is probably easier to use the Subscribe and Wait method than to use Callback Functions.

You can test your code by executing the following commands...

derror = turtle_waypoint(1,1)
derror = turtle_waypoint(10,1)
derror = turtle_waypoint(10,10)
derror = turtle_waypoint(1,10)

You should see something like this...

Exercise 3: Gazebo Simulation, Driving a Pioneer

Note: Before getting started, you should install a local copy of this ROS package: https://github.com/MobileRobots/amr-ros-config by doing the following...

cd ~/catkin_ws/src
git clone https://github.com/MobileRobots/amr-ros-config.git
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

Now open a new terminal so that ROS re-sources all the installed packages.  To make sure you have installed the correct packages, issue this command and check that the output is consistent.

bsb@humuhumu:~/catkin_ws$ rospack list | grep amr
amr_robots_description /home/bsb/catkin_ws/src/amr-ros-config/description
amr_robots_gazebo /home/bsb/catkin_ws/src/amr-ros-config/gazebo
amr_robots_launchfiles /home/bsb/catkin_ws/src/amr-ros-config/launch

You will also need the ROS package nre_p3at to setup a simulation in ROS.  Follow the directions on the Robotics Computation Wiki for Setting Up a ROS Package with Git.  (Remember, the Robotics Computation site is a shared wiki (gouge), so if you see ways to imporve the directions please contribute!)

You should be able to run this command to get an instance of Gazebo running with a Pioneer robot...

roslaunch nre_p3at p3at.gazebo.launch 

You should see something like this...

If this does not work we'll need to debug your ROS/Gazebo setup!

To get an idea of what is going on, use the introspection command-line tools (rostopic and rosnode) to list what nodes are running and the communications.

  • We can drive our simulated robot using velocity commands (geometry_msgs/Twist).  On what topic should we publish to command the robot?

Now, as we did in Assignment 2, we can use a script with a few "rostopic pub" commands to drive our robot around.  Create a new text file in your ROS package called /scripts/pdrive.sh.  Include commands in that file to drive your simulated pioneer.  Your pdrive.sh script should drive your Pioneer in a 5 m by 5m box, return to the origin and stop.  (This goal is approximate - you do not need to be exact with the dimensions.)

If you want to see where your robot has gone, the following are some options for viewing the odometry:

  • rostopic echo /sim_p3at/odom
  • rosrun rqt_topic rqt_topic
  • rosrun rqt_plot rqt_plot /sim_p3at/odom/pose/pose/position/x:y

Here is an image of using rqt_plot to show the position vs. time.  As you can see, my pdrive.sh script is not very accurate, but it is generally a 5x5 box.

To complete the exercise, make sure and add the /scripts/pdrive.sh to your repository - commit and push.

Exercise 4: MATLAB Control in Simulation

Now we are to the point where we can write a robot controller in MATLAB.  In this exercise you will write a MATLAB function called pcontrol(X,Y) in the file matlab/pcontrol.m .  You should use a Callback Functions that is called when new position information is published.   The callback function should determine a velocity command (linear and angular) based on the current position and the goal position - then publish that command to the appropriate topic.  When the simulated pioneer is within 0.5 m of the goal (X,Y), the function should return the error between the current odometry position and the goal.

You will need to generate two MATLAB files.  Skeletons of each are below to get you started, but with key parts missing...

  • The matlab/pcontrol.m file is the main function to drive the robot to a waypoint.

    pcontrol.m
    function derror = pcontrol(X,Y)
    
    global derror;  % This is the distance to the waypoint - calc. in callback
    derror = 10;  % Initially we'll make this large
    
    % Make a goal vector
    goal = [X,Y];  % Goal position in x/y
    
    % Setup publisher
    cmdpub = rospublisher('TOPIC',rostype.geometry_msgs_Twist);
    cmdmsg = rosmessage(cmdpub);
    
    % Setup subscription - which implemets our controller.
    % We pass the publisher, the message to publish and the goal as
    % additional parameters to the callback function.
    odomsub = rossubscriber('TOPIC',{@p3odomCallback,cmdpub,cmdmsg,goal});
    
    % Now just wait for the error to be less than the threshold
    % b/c the derror is a global, it is changed by the callback
    while derror > 0.5
        pause(0.5)
    end
    
    clear odomsub;  % Remove the subscription callback
    % Publish a message to make the robot stop
    % Fill this in as well...
    
    return;
  • The matlab/p3odomCallback.m file

    function p3odomCallback(~,message,cmdpub,cmdmsg,goal)
    % Waypoint control - this function is called when we receive an odom msg.
    
    global derror;  % so that we can tell the pcontrol function when we are done
    
    % Generate a simplified pose
    pos = message.Pose.Pose;
    quat = pos.Orientation;
    % From quaternion to Euler
    angles = quat2eul([quat.W quat.X quat.Y quat.Z]);
    theta = angles(1);
    % State vector
    pose = [pos.Position.X, pos.Position.Y, theta];  % X, Y, Theta
    
    % Distance to the goal
    derror = ?;
    fprintf('Current pose X=%4.2f, Y=%4.2f, Theta=%4.2f, derror=%4.2f\n',pose,derror);
    
    % Algorithm goes here...
    
    
    % Publish the velocity commands
    cmdmsg.Linear.X = ?;
    cmdmsg.Angular.Z = ?;
    send(cmdpub,cmdmsg);
    
    

Another working example for another robot is available here: Husky Control in Gazebo 
Feel free to reuse as much of that MATLAB code as you find useful.

You can test your code with these instructions:

  1. Start a new instance of Gazebo with the simulated Pioneer...

     roslaunch nre_p3at p3at.gazebo.launch 
  2. Execute the following commands in MATLAB

    rosinit
    derror = pcontrol(5,0)
    derror = pcontrol(5,5)
    derror = pcontrol(0,5)
    derror = pcontrol(0,0)
    rosshutdown

Your simulated pioneer should drive in a 5x5 box.

To complete the exercise, make sure and add the /matlab/pcontrol.m matlab/p3odomCallback.m files to your repository.  You may also have other *.m files (e.g., for the callback function).  You should add them as well.  Commit and push. 

  • No labels