Assignment 7: Virtual body and artificial potentials (VBAP), single USV (21-3)

References and Prerequisites

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

ROS

Review       REP-103: Standard Units of Measure and Coordinate Conventions, particularly Axis Orientation

MATLAB

Exchange Data with ROS Publishers and Subscribers, particularly the Subscribe Using Callback FunctionsMATLAB Callback DefinitionBrowse the MATLAB ROS examples on your machine at     The documentation for ROS in /usr/local/MATLAB/examples/ros/main/MATLAB is a bit sparse, so working examples are helpful.

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".  

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

Gazebo Coordinates

Our high-level guidance feedback will operate in the local Gazebo coordinate system.  The coordinates follow the REP-103 convention for ENU coordinates.  The origin of the coordinate system is show in the image below.

The initial pose of the CoRa vessel is defined in the    file...vorc_marina.launch

<!-- Initial USV location and attitude--> <arg name="x" default="10" /> <arg name="y" default="-372" /> <arg name="z" default="0.1" /> <arg name="P" default="0" /> <arg name="R" default="0" /> <arg name="Y" default="0" />

Exercise 1: Virtual Leader Rabbit Setup

Following the VBAP approach we need a virtual leader for cooperative network of vehicles to follow.  We'll call this virtual leader the   rabbit.

The rabbit is implemented as a standalone ROS node that is part of the mrc_examples ROS package.   The node works as follows:

The node publishes a message to describe the current location of the rabbit.geometry_msgs/PointStampedThe message is published on the topic    /rabbitThe following parameters are defined by the user:

The waypoints the rabbit visits.  This is set as a YAML config file as a list of 2D points (x and y), in Gazebo coordinates.  The rabbit visits each point in order and then cycles back to start the sequence again.The velocity the rabbit travels when moving between points - in m/s.The update rate of the rabbit node in Hz.

Working Example

A working example is included as a launch file.  You should be able to execute the following... 

roslaunch mrc_examples rabbit.launch

And then watch the published locations with...

rostopic echo /rabbit

Customize

We will want to customize the rabbit behavior by changing the parameters.

Copy the   file into your assignment package as mrc_examples/config/rabbit_waypoints.yaml ay21_hw7/config/my_rabbit_waypoints.yaml

Modify the waypoints in the yaml file so that they relay the following positions relative to the starting point of the CoRaStarting Point:   x0 = 10.0, y0 = -372.0

 dX = 40, dy=30

x y

x0 y0

x0+dX y0

x0+dX y0+dY

x0 y0+dy

You'll need to give numeric values in the yaml file. Copy the   file into your assignment package as  and mrc_examples/launch/rabbit.launch ay21_hw7/launch/my_rabbit.launchmake the following modifications.

Change the   tag so that the waypoints in are used. <rosparam> ay21_hw7/config/my_rabbit_waypoints.yaml Change the rabbit velocity to 3 m/s (~6 kts)

Here is a visual representation of the rabbit's waypoints:

1. 2.

Verify

Run your customized rabbit node (you don't need Gazebo, we are just verifying the rabbit functionality) with

roslaunch ay21_hw7 my_rabbit.launch

Record a bag fileLet the rabbit run through at least one cycle of the waypoints, the stop the recording and shutdown the rabbit node.Write a MATLAB script   to process the bag file and generate two graphsay21_hw7/matlab/rabbit_verify.m

A plot of the x vs. y positions of the rabbit.  Save as ay21_hw7/images/rabbit_xy.pngA plot with two subplots - upper subplot of x vs. time, lower subplot of y vs time.  Save ay21_hw7/images/rabbit_xy_time.png

Add your launch, YAML, MATLAB and image files to your repository, commit and push.  (Please do not add your .bag files to the git repository - instead just push these to your persistent storage.)

Exercise 2: Simulation Setup, Low-Level Control

From the previous assignment you should have a single launch file that starts the VORC marina Gazebo scenario and the low-level PID feedback:  ay21_h   w6/launch/cora_closedloop.launch

Copy this file over to your new ROS package (ay21_hw7/launch) so that you can run the command:

roslaunch ay21_hw7 cora_closedloop.launch 

to start the simulation and the feedback control with the appropriate PID gain values.

Exercise 3: VBAP Implementation

Using MATLAB, implement a single-leader, single-vehicle VBAP approach as    Here is a sketch of the activities of ay21_hw7/matlab/vbap_single.mthe node:

Subscribe to the    topic for the pose and velocity values of the USV./cora/sensors/p3dSubscribe to the   topic for the position of the rabbit./rabbitWhen the program receives an update of the pose/velocity...

the VBAP algorithm is executed to generate new setpoints for the low-level controller (surge and yaw-rate)publish the setpoint values from the VBAP algorithm to the low-level PID controller as a Twist message

Here is an image to show the interconnection of the MATLAB node to implement the algorithm.

MATLAB Callbacks

Because our MATLAB program needs to subscribe to two topics simultaneously, it is necessary to use asynchronous callbacks which will allow our program to do more than one thing at a time.  Here are some articles that discuss the difference between the implementations:

https://en.wikipedia.org/wiki/Callback_(computer_programming)http://www.cs.unc.edu/~dewan/242/s07/notes/ipc/node9.html

Review the MATLAB instructions on using a subscriber with a callback function: https://www.mathworks.com/help/ros/ref/subscriber.html

To get started, a working example is provided:

In the    repository,   directory, copy the following files into your    directory:mrc_examples matlab ay21_hw7/matlabvbap.m : The main script that initializes ROS, subscribes, publishes and calls the VBAP function.usv_odom_callback.m : The callback function executed when a new USV odometry (pose and velocity) message is received.vbap_slsv.m : Function called by    to execute the VBAP algorithm.vbap.m

Watch the .Video Overview of the VBAP SLSV Prototype ExampleWatch the Video on .MATLAB ROS Callback TroubleshootingAnother trick is that you may find a situation where you had to stop the MATLAB node but there is a persistent autopilot setupoint.  To set the commanded velocity back to zero you can use the    command:rostopic pub

rostopic pub /cora/cmd_vel geometry_msgs/Twist "linear: x: 0.0 y: 0.0 z: 0.0angular: x: 0.0 y: 0.0 z: 0.0"

Start by extending the example to add a new subscriber for the rabbit position information.  

VBAP Implementation

Finally we are ready to implement the VBAP algorithm.  The empty vbap_slsv.m  function provides a blank slate for converting the theory into MATLAB.

The details of the implementation and adaptation of the algorithm are provided in his Overleaf Document: https://www.overleaf.com/read/nrtcycgzqbdt

Deliverable

To complete the exercise you should have an implementation of the VBAP algorithm in MATLAB that runs without error, subscribing to the two topics (rabbit and p3d navigation)  and publishing to the cmd_vel topic.  The algorithm does not need to be tuned to perform particularly well - we'll work on that next week.

1. 2. 3.

1. 2. 3.

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 Exercise 1 - but with the additional overlay of the x/y positions of the USV.

Troubleshooting - Static Rabbit

For debugging purposes, it may be enlightening to have the rabbit be stationary at a given position to see the response of the USV with the VBAP algorithm.  To do this, don't start the dedicated rabbit node - instead publish a position directly to the rabbit topic from the command line:

rostopic pub -r 10 /rabbit geometry_msgs/PointStamped "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: ''point: x: 30.0 y: -372.0 z: 0.0"

Troubleshooting - Visualizing the Rabbit Location in Gazebo

It may be helpful for debugging and tuning the VBAP implementation to be able to see a representation of the rabbit/virtual-leader in gazebo.

This functionality was added on 14 May 2021, so make sure to update your local copy of the mrc_examples repository...

git pull origin main

A working example is provided that does three things:

Starts the rabbit node described above.Spawns a visual marker, a red sphere with 1 m radius, in Gazebo - named "rabbit"Starts a new node that subscribes to the messages (from the rabbit node) and re-publishes the information as a geometry_msgs/PointStamped ga

message to set the position of the "rabbit" visual model in Gazebo.zebo_msgs/ModelState

roslaunch mrc_examples rabbit_w_visual.launch

Here is video to demonstrate: Gazebo Visualization of ROS Position

And here is an image:

Summary

Coming soon