In the last tutorial, we understood the data structure of UWB messages and how to visualize the data using a web application. In this tutorial, we will simulate UWB data for our vessel.
After completing this tutorial, you will be able to:
Subscribe to the odometry topic and publish UWB data
Calculate the position of the UWB sensor in the global frame
Develop a node that simulates UWB data based on the vessel’s motion
20.1 Getting Started
We will be using the same repository as the previous tutorial. Once the previous tutorial deadline is over, your instructor will update the upstream repository with the necessary files for this tutorial and raise a pull request on your team repository. You will need to pull in the changes from the upstream repository to your team repository to start this tutorial. The instructions to pull in the changes are given in Appendix B.
After you complete the steps shown in Appendix B to synchronize your repository with the upstream repository, navigate to the root directory of your repository. Pull the latest changes made to your team repository onto your local machine.
cd <your-repository-name>
git pull origin main
The relevant folder structure of the repository for this tutorial is shown below.
All your group members can work independently on their own branches. However, you will have to merge your branches back to the main branch before the end of the tutorial. We learnt how to do this in Chapter 14 using pull requests.
First start docker desktop and then follow the instructions below to run the course docker container. Make sure to navigate in the terminal to the root folder of your repository.
For Windows:
./ros2_run_devdocker.bat
For Ubuntu (or MacOS):
./ros2_run_devdocker.sh
Source the ROS 2 environment by running the following command:
sros2
Notice that this is an alias for sourcing your ROS 2 workspace as well as the ROS 2 installation in the docker container. Remember that every time you perform a colcon build on your workspace, you need to source the ROS 2 environment again with the command above.
20.2 Create a new ROS2 package
Create a new ROS2 package inside /workspaces/mavlab/ros2_ws/src/student/tut_06
cd /workspaces/mavlab/ros2_ws/src/student/tut_08ros2 pkg create --build-type ament_python mav_uwb_simulator --dependencies rclpy std_msgs nav_msgs sensor_msgs geometry_msgs --license MIT
20.3 Create a new node
Create a new node inside /workspaces/mavlab/ros2_ws/src/student/tut_08/mav_uwb_simulator/mav_uwb_simulator/
cd /workspaces/mavlab/ros2_ws/src/student/tut_08/mav_uwb_simulator/mav_uwb_simulatortouch mav_uwb_simulator.py
Copy the following code into the mav_uwb_simulator.py file:
Code
import rclpyfrom rclpy.node import Nodefrom nav_msgs.msg import Odometryfrom geometry_msgs.msg import PoseWithCovarianceStamped, PoseArray, Poseimport numpy as npimport syssys.path.append('/workspaces/mavlab/')from ros2_ws.src.student.tut_03.read_input import read_inputfrom ros2_ws.src.student.tut_03.class_vessel import Vesselfrom ros2_ws.src.student.tut_03.module_kinematics import eul_to_rotm, quat_to_eulclass MavUwbSimulator(Node):def__init__(self):super().__init__('mav_uwb_simulator')# Read vessel parameters and create vessel instance vessel_params, hydrodynamic_data = read_input()self.vessel = Vessel(vessel_params, hydrodynamic_data)# Find UWB sensors and create publishersself.uwb_sensors = []self.uwb_publishers = []self.uwb_timers = []self.vessel_state = np.zeros(13) count =0for sensor in vessel_params['sensors']:if sensor['sensor_type'] =='UWB': position_covariance = np.array(sensor['noise'][0]['position_covariance']) if'noise'in sensor else np.eye(3) *0.01self.uwb_sensors.append({'id': count,'position': np.array(sensor['sensor_location']),'orientation': np.array(sensor.get('sensor_orientation', [0.0, 0.0, 0.0])),'topic': sensor['topic'],'rate': sensor['rate'],'position_covariance': position_covariance })# Create publisher for this UWB sensor's positionself.uwb_publishers.append(self.create_publisher(PoseWithCovarianceStamped, self.uwb_sensors[count]['topic'], 10) )# Create timer for this UWB sensorself.uwb_timers.append(self.create_timer(1/self.uwb_sensors[count]['rate'],lambda i=count: self.publish_uwb_position(i) ) ) count +=1# Subscribe to odometryself.subscription =self.create_subscription( Odometry,'/mav_odom',self.odom_callback,10 )self.get_logger().info('UWB Simulator initialized with {} sensors'.format(len(self.uwb_sensors)))def compute_uwb_position(self, vessel_state, sensor_position):""" Compute UWB sensor position in global frame Args: vessel_state: 13x1 vessel state vector sensor_position: 3x1 sensor position vector in body frame Returns: position: 3x1 position vector in global frame """# Extract vessel position and orientation vessel_position = vessel_state[6:9] vessel_orientation = vessel_state[9:12]# Initialize sensor position in global frame sensor_global_position = np.zeros(3)#===========================================================================# TODO: Calculate sensor position in global frame#===========================================================================# Write your code herepass#===========================================================================return sensor_global_positiondef odom_callback(self, msg):"""Handle incoming odometry messages"""# Extract state from odometry messageself.vessel_state = np.zeros(13)self.vessel_state[0:3] = [msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z]self.vessel_state[3:6] = [msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z]self.vessel_state[6:9] = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]# Extract Euler angles from quaternionself.vessel_state[9:12] = quat_to_eul([msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z])def publish_uwb_position(self, i):"""Publish UWB sensor position with covariance""" sensor =self.uwb_sensors[i]# Create position message pose_msg = PoseWithCovarianceStamped() pose_msg.header.stamp =self.get_clock().now().to_msg() pose_msg.header.frame_id ='map'# Compute sensor position and orientation in global frame position =self.compute_uwb_position(self.vessel_state, sensor['position'] )# Add noise to position measurement position_noise = np.zeros(3)# Generate correlated noise using covariance matrix position_noise = np.random.multivariate_normal( mean=np.zeros(3), cov=sensor['position_covariance'] ) noisy_position = position + position_noise# Set position and orientation in message pose_msg.pose.pose.position.x =float(noisy_position[0]) pose_msg.pose.pose.position.y =float(noisy_position[1]) pose_msg.pose.pose.position.z =float(noisy_position[2])# Set covariance - convert 3x3 to 6x6 (only position components used) pose_covariance = np.zeros((6, 6)) pose_covariance[0:3, 0:3] = sensor['position_covariance'] pose_msg.pose.covariance = pose_covariance.flatten().tolist()# Publish positionself.uwb_publishers[i].publish(pose_msg)def main(args=None): rclpy.init(args=args) uwb_simulator = MavUwbSimulator() rclpy.spin(uwb_simulator) uwb_simulator.destroy_node() rclpy.shutdown()if__name__=='__main__': main()
Let’s break down the code in the mav_uwb_simulator.py file. This node subscribes to the odometry data from the vessel simulator and publishes the UWB data to the /makara_00/uwb_00 topic. The odometry data provides the position, orientation, linear velocity and angular velocity of the vessel. The position and orientation of the UWB sensor need not necessarily coincide with the origin of the BCS. Similarly the sensor also need not be oriented with its frame of reference parallel to the BCS. Therefore, the measurements of the UWB need to be computed based on the position and orientation of the UWB sensor with respect to the BCS.
If you observe, the input file (originally created in Chapter 15) located in tut_03 folder has been modified in the previous tutorial to include the UWB sensor parameters. The sensors list in the input file now contains the parameters for the different sensors that the vessel is equipped with. In this tutorial we are only interested in the UWB sensor. The sensors list in the input file now contains the following additional lines:
These lines describe the sensor parameters for the IMU and the UWB sensor.
20.3.1 Implementation Tasks
There is only one task to complete in this tutorial. In this task, the vessel state vector is assumed to be \(13 \times 1\) vector as defined in Chapter 15. The first three elements denote the components of the linear velocity of the vessel in the BCS. The next three elements denote the components of the angular velocity of the vessel in the BCS. The next three elements denote the position of the vessel in the GCS. The next three elements denote the Euler angles of the vessel in the GCS. The last element denotes the actual rudder angle.
In addition to the state vector, the functions also take the sensor position as input. The sensor position is a \(3 \times 1\) vector denoting the position of the sensor in the BCS.
Task 1: Complete the compute_uwb_position function
Location:mav_uwb_simulator.py, compute_uwb_position() function
Description: The compute_uwb_position function computes the position of the UWB sensor in the global frame.
Input:
vessel_state: 13x1 vessel state vector
sensor_position: 3x1 sensor position vector
Output:
position: 3x1 position vector in global frame
20.4 Create a launch file
Create a launch file inside /workspaces/mavlab/ros2_ws/src/student/tut_08/mav_uwb_simulator/mav_uwb_simulator/launch
cd /workspaces/mavlab/ros2_ws/src/student/tut_08/mav_uwb_simulator/mkdir launchcd launchtouch mav_uwb_simulator_launch.py
Copy the following code into the mav_uwb_simulator_launch.py file:
Code
from launch import LaunchDescriptionfrom launch_ros.actions import Nodefrom launch.actions import ExecuteProcessfrom launch.substitutions import PathJoinSubstitutionfrom launch_ros.substitutions import FindPackageSharefrom launch.actions import IncludeLaunchDescriptionfrom launch.launch_description_sources import AnyLaunchDescriptionSourcedef generate_launch_description():# Path to the rosbridge launch file rosbridge_launch = PathJoinSubstitution([ FindPackageShare('rosbridge_server'),'launch','rosbridge_websocket_launch.xml' ])# Path to the built web directory web_build_dir = PathJoinSubstitution([ FindPackageShare('mav_uwb_visualizer'),'web' ])return LaunchDescription([# Start the HTTP server to serve web files ExecuteProcess( cmd=['python3', '-m', 'http.server', '8000', '--directory', web_build_dir], name='http_server', output='screen' ), Node( package='mav_simulator', executable='mav_simulate', name='mav_simulate', output='screen' ), Node( package='mav_simulator', executable='mav_control', name='mav_control', output='screen' ), Node( package='mav_uwb_simulator', executable='mav_uwb_simulator', name='mav_uwb_simulator', output='screen' ), IncludeLaunchDescription( AnyLaunchDescriptionSource(rosbridge_launch) ) ])
Let’s break down the code in the launch file. This launch file is similar to the launch file we developed in Chapter 18. In this launch file, we are starting the mav_simulate and mav_control nodes using the package we built in Chapter 16. This will start the vessel simulator and the controller. The simulator will publish the odometry data to the /mav_odom topic. The controller will subscribe to the odometry data and publish the control commands to the /mav_rudder_command topic. In addition to this, the launch file also starts a ROS bridge server and a HTTP server to serve the web files for the web application developed in Chapter 19. The ROS bridge server will be used to communicate the data being published on the topic /makara_00/uwb_00 to the web application. Finally, the launch file starts the mav_uwb_simulator node which will subscribe to the odometry data from the simulator and publish the UWB data to the /makara_00/uwb_00 topic. Unlike in the last tutorial where we visualized the UWB data from a ros bag file, in this tutorial we will visualize the UWB data from the UWB simulator we develop in this tutorial.
In order for the launch file to work, we need to make sure that the setup.py file for the mav_uwb_simulator package is updated. Navigate to the setup.py file:
cd /workspaces/mavlab/ros2_ws/src/student/tut_08/mav_uwb_simulator
Overwrite the setup.py file with the following code:
You should see an output similar to the following:
[INFO] [launch]: All log files can be found below /home/mavlab/.ros/log/2025-03-11-00-09-07-924772-5116014b0850-6363
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [http_server-1]: process started with pid [6364]
[INFO] [mav_simulate-2]: process started with pid [6366]
[INFO] [mav_control-3]: process started with pid [6368]
[INFO] [mav_uwb_simulator-4]: process started with pid [6370]
[INFO] [rosbridge_websocket-5]: process started with pid [6372]
[INFO] [rosapi_node-6]: process started with pid [6374]
[mav_control-3] [INFO] [1741651748.738997292] [mav_control]: MAV Controller initialized
[rosbridge_websocket-5] [INFO] [1741651748.800955780] [rosbridge_websocket]: Rosbridge WebSocket server started on port 9090
[mav_uwb_simulator-4] [INFO] [1741651748.926255838] [mav_uwb_simulator]: UWB Simulator initialized with 1 sensors
[mav_control-3] [INFO] [1741651749.048518203] [mav_control]:
[mav_control-3] Time: 0.33s
[mav_control-3] Rudder Command: 5.00 deg
[mav_control-3]
[mav_simulate-2] [INFO] [1741651749.086385235] [mav_simulate]:
[mav_simulate-2] Time: 0.10s
[mav_simulate-2] Position (x,y,z): [0.05, 0.00, 0.00]
[mav_simulate-2] Euler angles (phi,theta,psi): [0.00, 0.00, 0.00]
[mav_simulate-2] Velocity (u,v,w): [0.50, 0.00, 0.00]
[mav_simulate-2] Angular velocity (p,q,r): [0.00, 0.00, 0.00]
[mav_simulate-2] Rudder Command: 0.00 deg
[mav_simulate-2]
[mav_simulate-2] [INFO] [1741651749.147082338] [mav_simulate]:
[mav_simulate-2] Time: 0.20s
[mav_simulate-2] Position (x,y,z): [0.10, 0.00, 0.00]
[mav_simulate-2] Euler angles (phi,theta,psi): [0.00, 0.00, -0.01]
[mav_simulate-2] Velocity (u,v,w): [0.50, 0.00, 0.00]
[mav_simulate-2] Angular velocity (p,q,r): [0.00, 0.00, -0.00]
[mav_simulate-2] Rudder Command: 3.16 deg
[mav_simulate-2]
[mav_control-3] [INFO] [1741651749.148035293] [mav_control]:
[mav_control-3] Time: 0.43s
[mav_control-3] Rudder Command: 5.00 deg
[mav_control-3]
You can access the web application by opening the following URL in your web browser:
http://localhost:8000/
20.7 Evaluation
The evaluation of the tutorial will be performed in the classroom. Notice that this is a group activity. You will be working in a group of 4 students. The evaluation will be performed on the group level. In order to pass the tutorial, the group must pass all the tests in the GitHub Actions workflow and also be able to answer all the questions during the evaluation in the classroom. Those who are not present in the classroom on the day of evaluation will not be able to participate in the evaluation and will automatically fail the tutorial.
While one of the objectives of these tutorials is to give you hands on experience with coding and practical implementation, the more important goal is to help you relate what you are doing in your tutorials to the underlying concepts discussed in the class. Therefore, while you are working on the implementation tasks, make sure you are also thinking about the underlying concepts and how they are being applied in the code.
Some questions to think about while you are working on the implementation tasks:
What is the difference between the position reported by the UWB and the position of the vessel in the vessel state vector? How do we derive the former from the latter?
How do you think we obtained the covariance values for the UWB measurements?
Are we adding noise to our measurements?
20.8 Instructor Feedback through Pull Requests
Commit your changes to the remote repository often. This will help you to track your progress and also help you to revert to a working version if you make a mistake. In addition, the instructor and TA will be able to provide you with feedback on your code through the Feedback pull request available in your repository. Please do not close this pull request.