1. 2. 3.

Assignment 8: VBAP, single USV tuning and multiple (2) USVs (21-3)

References and Prerequisites

You're expected to work through (actually do the tutorials on your computer!) before doing the exercises below!

ROS

Names:  Read the following documentation.  Names will become very important when dealing with multiple robots.  We will use ROS Namespacesto compartmentalize the ROS network for each robot.

ROS Names - documentationA Gentle Introduction to ROS - a Chapter 5: Graph Resource Names Roslaunch tag - notice the namespace (ns) attribute <group> 

Cloudsim

Persistent Storage: Setting Executable Bit / Permissions - for convenience.

Waypoint Guidance

The book Robotics, Vision and Control, by Peter Corke  is available electronically through the NPS library 

Go to the library website, search for the title, and follow the links to download a PDF of the bookSee Chapter 4: Mobile Robot Vehicles

Section 4.2.1, Moving to a Point describes the waypoint guidance method we will use.

Goal

The objective of this assignment is to implement a potential field based guidance algorithm based on the virtual body and artificial potentials (VBAP) described in "Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment".  This week we should be able to...

Tune the single-leader, single-vehicle MATLAB implementation.Setup a two-vehicle case with low-level control for each USV.Extend our VBAP implementation for a "Collective" scenario, where each vehicle is attracted to the virtual leader (rabbit), but now aware of the other vehicles.

4.

1. 2.

Next Week: Extend the VBAP implementation to a "Cooperative" scenario, where the vehicles attempt to maintain a safe distance from each other.

Source: Parker, Distributed Intelligence

Assignment Setup

Assignment on Github

As with Assignment 1, this assignment will be submitted as a branch of the github repository for the assignment in the MultiRobotControl organization: https://github.com/MultiRobotControlEach student should work only in their branch of the repository.

Clone your repository to the src directory within your catkin workspace and make it a ROS package.  Add the CMakelists.txt  and package.xml  files to your git repository.

Update Packages

There are new additions to the    package, so you will need to  the latest changes from the github remote to your local copy.mrc_examples pull

# Move into the directory where you have mrc_examplescd catkin_ws/src/mrc_examples# Pull changes from remote (origin) to your local copygit pull origin main

Exercise 1: VBAP SLSV Tuning

An oft quoted approach to developing software that applies to many other development scenarios, such as developing control systems, is Make it work, (see the associated article, )., . make it right, make it fast Kent Beck "Coding faster: Make it work, then make it good"

In the previous assignment you should have made the VBAP SLSV algorithm work - now we'll work on making it right/fast/good.

Setup

Copy over the pertinent MATLAB files from the previous exercise to your new repository.  In    put the following files:ay21_hw8/matlab

vbap.m - The main script/program to setup the callbacks, call the algorithm, etc.vbap_slsv.m - Implementation of the single leader, single vehicle VBAP algorithm as a function.rabbit_callback.m - Callback function for virtual leader position subscriber.usv_odom_callback.m - Callback function for vehicle navigation subscriber.

Create a new launch file:    that is based on the    from the previous ay21_hw8/launch/vbap_slsv.launch cora_closedloop.launchassignment.  This launch file should do the following:

Include the    to start Gazebo with the marina world and spawn our single CoRa USV.mrc_examples/launch/vorc_marina.launchStart the    node with your PID gain values from previous assignments.  Or, you can use these gain values.usv_control_diff_drive.py

<!-- Surge --> <param name="velKp" value="1.1"/>

2.

3.

4.

<param name="velKi" value="0.0"/> <param name="velKd" value="0.0"/> <!-- Yaw Rate --> <param name="yawKp" value="2.5"/> <param name="yawKi" value="1.0"/> <param name="yawKd" value="0.0"/>

Spawn the model described by the   description (see  mrc_examples/urdf/rabbit.urdf mrc_examples/launch/rabbit_w_visual.  for an example)launch

Start the   node to connect the rabbit publication to update the red sphere rabbit visualization  (again see  pointstamped2modelstate.py mrc_  for an example)examples/launch/rabbit_w_visual.launch

You should have  a scenario similar to the image below.  Note that the rabbit starts at (0, 0) which is top-center of the image.

Modify Algorithm and Scenario

To simplify the virtual leader portion of the algorithm, we will use the Moving to a Point algorithm from P. Corke discussed above.  This is simpler to tune as it only has two gain parameters:

k_h: The yaw-rate (heading) gaink_v: The surge gain (velocity) gain.

Video Examples, Tuning and Debugging:

Moving the rabbit using rostopic pub, turning off control output to verify outputMoving the USV (instead of the rabbit), translation and rotation, tuning the heading portion of the waypoint guidanceTuning the surge component of waypoint guidance with stationary virtual leader.Running the rabbit to verify waypoint guidance.Following the rabbit - two scenarios - 8x speedup

Change the rabbit waypoints to a 100x100 m box:

1. 2. 3.

# 2D Array of waypointswaypoints: - [10.0 , -372.0, 0.0] - [110.0 , -372.0, 0.0] - [110.0 , -272.0, 0.0] - [10.0 , -272.0, 0.0]

Deliverable

When you have a working version...

Record a bag file that inlcudesThe rabbit positionsThe USV odometry (pose and velocity)The commanded twist message sent to the low-level controller.

Process the bag file with MATLAB to generate the same figures from the previous assignment, i.e.,A plot of the x vs. y positions of the rabbit and the USV.  Save as ay21_hw8/images/vbap_slsv_xy.pngA plot with two subplots - upper subplot of x (rabbit and USV) vs. time, lower subplot of y (rabbit and USV) vs time.  Save ay21_hw8/images/vbap_slsv_xy_time.png

Exercise 2: Multiple (N=2) USVs with Low-Level Control

Working Example

As a starting point, you should be able to run the following launch file

roslaunch mrc_examples marina_2x.launch verbose:=true

To generate a simulation scenario described by the following two images:

Add Low-Level (Autopilot) Control for Each USV

Copy the working example file into your assignment repository as   ay21_hw8/launch/marina_2x.launch

Edit the launch file to

Start two instances of the usv_control_diff_drive.py node, with your PID gains, within the namespace of each USV (cora1 and cora2)Spawn the model described by the   description (see  mrc_examples/urdf/rabbit.urdf mrc_examples/launch/rabbit_w_visual.

  for an example)launchStart the   node to connect the rabbit publication to update the red sphere rabbit visualization  (again see  pointstamped2modelstate.py mrc_

  for an example)examples/launch/rabbit_w_visual.launch

Exercise 3:  Collective VBAP Now that we have two independent USV's in the simulation, cora1 and cora2, we will extend the MATLAB implementation of the VBAP algorithm to service two USVs simultaneously. To start with the two vessels will independently pursue the virtual leader / rabbit and make no effort to avoid each other.

Start with the    program you created for the SLSV case.  Copy it as    The program should...matlab/vbap.m matlab/vbap_2x.mSubscribe to the navigation information for both USVs.  (You can create two separate callback functions, or have both subscribers call the same callback function.)Subscribe to the rabbit position informationRun the VBAP algorithm for each USV to have each vessel follow the rabbit.  The VBAP algorithm should be unchanged from Exercise 1Publish Twist commands to both USVs

The final architecture should look something like this...

.Video: VBAP, single leader, two vehicle, collective demonstration at 4x speed

Once you have working solution, 

record a bag file with position information for both USVs and the rabbit, e.g.,

rosbag record /cora1/cora/sensors/p3d /cora2/cora/sensors/p3d /rabbit /cora1/cora/cmd_vel /cora2/cora/cmd_vel

Process the bag file with MATLAB to generate the same figures from the previous assignment, i.e.,A plot of the x vs. y positions of the rabbit and both USVs.  Save as ay21_hw8/images/vbap_sltv_xy.pngA plot with two subplots - upper subplot of x (rabbit and both USVs) vs. time, lower subplot of y (rabbit and USV) vs time.  Save ay21_hw8/images/vbap_sltv_xy_time.png