Assignment 7: Virtual body and artificial potentials (VBAP ...
Transcript of Assignment 7: Virtual body and artificial potentials (VBAP ...
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