diff --git a/AirSim.sln b/AirSim.sln
index 7acb8e4a5..602d473ad 100644
--- a/AirSim.sln
+++ b/AirSim.sln
@@ -9,6 +9,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AirLib", "AirLib\AirLib.vcx
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HelloDrone", "HelloDrone\HelloDrone.vcxproj", "{98BB426F-6FB5-4754-81BC-BB481900E135}"
EndProject
+
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HelloRov", "HelloRov\HelloRov.vcxproj", "{98BB426F-6FB5-4754-81BC-BB481900E135}"
+EndProject
+
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MavLinkCom", "MavLinkCom\MavLinkCom.vcxproj", "{8510C7A4-BF63-41D2-94F6-D8731D137A5A}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DroneServer", "DroneServer\DroneServer.vcxproj", "{A050F015-87E2-498F-866A-2E5AF65AF7AC}"
diff --git a/AirSim.sln.vlconfig b/AirSim.sln.vlconfig
index 329a1b5e5..d947b97fe 100644
--- a/AirSim.sln.vlconfig
+++ b/AirSim.sln.vlconfig
@@ -86,6 +86,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/HelloRov/HelloRov.vcxproj.filters b/HelloRov/HelloRov.filters
similarity index 100%
rename from HelloRov/HelloRov.vcxproj.filters
rename to HelloRov/HelloRov.filters
diff --git a/HelloRov/HelloRov.vcxproj b/HelloRov/HelloRov.vcxproj
index 38cd7e70a..278012df9 100644
--- a/HelloRov/HelloRov.vcxproj
+++ b/HelloRov/HelloRov.vcxproj
@@ -44,7 +44,7 @@
{98BB426F-6FB5-4754-81BC-BB481900E135}
Win32Proj
- HelloDrone
+ HelloRov
@@ -170,7 +170,7 @@
$(ProjectDir)..\AirLib\deps\rpclib\include;include;$(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include
true
/w34263 /w34266 %(AdditionalOptions)
- 4100;4505;4820;4464;4514;4710;4571;4244%(DisableSpecificWarnings)
+ 4100;4505;4820;4464;4514;4710;4571;%(DisableSpecificWarnings)
stdcpp17
diff --git a/HelloRov/buildcommand.txt b/HelloRov/buildcommand.txt
deleted file mode 100644
index e69de29bb..000000000
diff --git a/HelloRov/main.cpp b/HelloRov/main.cpp
index 4cf6ab995..4f68b6c92 100644
--- a/HelloRov/main.cpp
+++ b/HelloRov/main.cpp
@@ -1,153 +1,118 @@
-// Copyright (c) Microsoft Corporation. All rights reserved.
-// Licensed under the MIT License.
+/*
+ Circular Trajectory Following Controller
+
+ This program implements a controller for an ROV (Remotely Operated Vehicle) to follow a circular trajectory.
+ It utilizes the AirSim API for communication with the ROV, and Eigen library for mathematical operations.
+
+ Circular trajectory parameters:
+ - Radius of the circle: 1.0
+ - Angular velocity: 0.05 radians per second
+ - Fixed altitude: 1.0
+ - Fixed yaw angle: 0.0
+
+ The controller generates a reference vector for the ROV based on the circular trajectory parameters.
+ It calculates the control signal (wrench) using a proportional (P) controller to minimize the error between the reference
+ position and the actual ROV state. The control signal is then converted into PWM signals for the motors
+ and sent to the ROV for execution.
+
+ Author: Hakim Amer
+ Date: 22/4/2024
+*/
+
-#include "common/common_utils/StrictMode.hpp"
-STRICT_MODE_OFF
-#ifndef RPCLIB_MSGPACK
-#define RPCLIB_MSGPACK clmdep_msgpack
-#endif // !RPCLIB_MSGPACK
-#include "rpc/rpc_error.h"
-STRICT_MODE_ON
#include "vehicles/rov/api/RovRpcLibClient.hpp"
#include "common/common_utils/FileSystem.hpp"
-#include
-#include
+#include "rov_control.hpp"
+
+
+
+// Function to generate reference vector for circular trajectory
+Eigen::Vector4d generateReference(double time)
+{
+ // Define circular trajectory parameters
+ double radius = 1.0; // Radius of the circle
+ double angular_velocity = 0.05; // Angular velocity (radians per second)
+
+ // Compute reference position on the circular trajectory
+ double ref_x = radius * cos(angular_velocity * time);
+ double ref_y = radius * sin(angular_velocity * time);
+ double ref_z = 1.0; // Fixed altitude
+ double ref_yaw = 0.0; // Fixed yaw angle
+
+ // Construct and return the reference vector
+ return Eigen::Vector4d(ref_x, ref_y, ref_z, ref_yaw);
+}
+
+
+
+
+
+msr::airlib::vector calculatePWM(Eigen::VectorXd wrench)
+{
+ // Define A matrix
+ Eigen::Matrix A;
+ A << 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, -0.5, -0.5, //Thrust allocation matrix of bluerov2 heavy
+ 0.0, 0.0, 0.0, 0.0, -0.5, 0.5, 0.5, -0.5,
+ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.1, -0.1, 0.1, -0.1;
+
+ std::vector pwm_out(8);
+
+
+ // Construct vector v from wrench values
+ Eigen::Vector4d v;
+ v << wrench(0), wrench(1), wrench(2), wrench(3);
+
+
+ Eigen::MatrixXd pwm_vector = (A.transpose() * A).ldlt().solve(A.transpose() * v);
+
+ pwm_vector /= 40.0; //scale pwm signal
+
+ msr::airlib::vector vect{ static_cast(pwm_vector(0)),
+ static_cast(pwm_vector(1)),
+ static_cast(pwm_vector(2)),
+ static_cast(pwm_vector(3)), static_cast(pwm_vector(4)),
+ static_cast(pwm_vector(5)), static_cast(pwm_vector(6)),
+ static_cast(pwm_vector(7)) };
+
+ //msr::airlib::vector vect{ -0.5f, -0.5f, -0.5f, -0.5f, 0.05f, 0.05f, 0.05f, 0.05f };
+
+ return vect;
+}
int main()
{
- using namespace msr::airlib;
msr::airlib::RovRpcLibClient client;
-
- typedef ImageCaptureBase::ImageRequest ImageRequest;
- typedef ImageCaptureBase::ImageResponse ImageResponse;
- typedef ImageCaptureBase::ImageType ImageType;
- typedef common_utils::FileSystem FileSystem;
-
- /*
- try {
- client.confirmConnection();
-
- std::cout << "Press Enter to get FPV image" << std::endl;
- std::cin.get();
- vector request = { ImageRequest("0", ImageType::Scene), ImageRequest("1", ImageType::DepthPlanar, true) };
- const vector& response = client.simGetImages(request);
- std::cout << "# of images received: " << response.size() << std::endl;
-
- if (response.size() > 0) {
- std::cout << "Enter path with ending separator to save images (leave empty for no save)" << std::endl;
- std::string path;
- std::getline(std::cin, path);
-
- for (const ImageResponse& image_info : response) {
- std::cout << "Image uint8 size: " << image_info.image_data_uint8.size() << std::endl;
- std::cout << "Image float size: " << image_info.image_data_float.size() << std::endl;
-
- if (path != "") {
- std::string file_path = FileSystem::combine(path, std::to_string(image_info.time_stamp));
- if (image_info.pixels_as_float) {
- Utils::writePFMfile(image_info.image_data_float.data(), image_info.width, image_info.height, file_path + ".pfm");
- }
- else {
- std::ofstream file(file_path + ".png", std::ios::binary);
- file.write(reinterpret_cast(image_info.image_data_uint8.data()), image_info.image_data_uint8.size());
- file.close();
- }
- }
- }
- }
- */
-
-
-
- client.enableApiControl(true);
- client.armDisarm(true);
-
- auto barometer_data = client.getBarometerData();
- std::cout << "Barometer data \n"
- << "barometer_data.time_stamp \t" << barometer_data.time_stamp << std::endl
- << "barometer_data.altitude \t" << barometer_data.altitude << std::endl
- << "barometer_data.pressure \t" << barometer_data.pressure << std::endl
- << "barometer_data.qnh \t" << barometer_data.qnh << std::endl;
-
- auto imu_data = client.getImuData();
- std::cout << "IMU data \n"
- << "imu_data.time_stamp \t" << imu_data.time_stamp << std::endl
- << "imu_data.orientation \t" << imu_data.orientation << std::endl
- << "imu_data.angular_velocity \t" << imu_data.angular_velocity << std::endl
- << "imu_data.linear_acceleration \t" << imu_data.linear_acceleration << std::endl;
-
- auto gps_data = client.getGpsData();
- std::cout << "GPS data \n"
- << "gps_data.time_stamp \t" << gps_data.time_stamp << std::endl
- << "gps_data.gnss.time_utc \t" << gps_data.gnss.time_utc << std::endl
- << "gps_data.gnss.geo_point \t" << gps_data.gnss.geo_point << std::endl
- << "gps_data.gnss.eph \t" << gps_data.gnss.eph << std::endl
- << "gps_data.gnss.epv \t" << gps_data.gnss.epv << std::endl
- << "gps_data.gnss.velocity \t" << gps_data.gnss.velocity << std::endl
- << "gps_data.gnss.fix_type \t" << gps_data.gnss.fix_type << std::endl;
-
- auto magnetometer_data = client.getMagnetometerData();
- std::cout << "Magnetometer data \n"
- << "magnetometer_data.time_stamp \t" << magnetometer_data.time_stamp << std::endl
- << "magnetometer_data.magnetic_field_body \t" << magnetometer_data.magnetic_field_body << std::endl;
- // << "magnetometer_data.magnetic_field_covariance" << magnetometer_data.magnetic_field_covariance // not implemented in sensor
-
-
- float takeoff_timeout = 5;
- client.takeoffAsync(takeoff_timeout)->waitOnLastTask();
-
- // switch to explicit hover mode so that this is the fall back when
- // move* commands are finished.
- std::this_thread::sleep_for(std::chrono::duration(5));
- client.hoverAsync()->waitOnLastTask();
-
-
- // moveByVelocityZ is an offboard operation, so we need to set offboard mode.
- client.enableApiControl(true);
-
- auto position = client.getRovState().getPosition();
- //float z = position.z(); // current position (NED coordinate system).
- const float speed = 3.0f;
- const float size = 10.0f;
- const float duration = size / speed;
- // DrivetrainType drivetrain = DrivetrainType::ForwardOnly;
- YawMode yaw_mode(true, 0);
-
- vector vect{ 0.1f, 0.1f, 0.1f, 0.1f, 0.05f, 0.05f, 0.05f, 0.05f };
- while (true) {
- client.moveByMotorPWMsAsync(vect, duration);
- }
- /*
- std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl;
- client.moveByVelocityZAsync(speed, 0, z, duration, drivetrain, yaw_mode);
- std::this_thread::sleep_for(std::chrono::duration(duration));
- std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl;
- client.moveByVelocityZAsync(0, speed, z, duration, drivetrain, yaw_mode);
- std::this_thread::sleep_for(std::chrono::duration(duration));
- std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl;
- client.moveByVelocityZAsync(-speed, 0, z, duration, drivetrain, yaw_mode);
- std::this_thread::sleep_for(std::chrono::duration(duration));
- std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl;
- client.moveByVelocityZAsync(0, -speed, z, duration, drivetrain, yaw_mode);
- std::this_thread::sleep_for(std::chrono::duration(duration));
- *
- client.hoverAsync()->waitOnLastTask();
-
- std::cout << "Press Enter to land" << std::endl;
- std::cin.get();
- client.landAsync()->waitOnLastTask();
-
- std::cout << "Press Enter to disarm" << std::endl;
- std::cin.get();
- client.armDisarm(false);
- }
- catch (rpc::rpc_error& e) {
- std::string msg = e.get_error().as();
- std::cout << "Exception raised by the API, something went wrong." << std::endl
- << msg << std::endl;
+ client.enableApiControl(true);
+ client.armDisarm(true);
+ client.confirmConnection();
+ double time = 0.0; // Initialize time variable
+
+ while (true) {
+ // Generate reference vector based on time
+ Eigen::Vector4d reference = generateReference(time);
+
+ // Create an instance of RovControl with the generated reference
+ RovControl rov_controller(client, reference);
+
+ // Call the calculate_wrench method to get the control signal
+ Eigen::Vector4d wrench = rov_controller.calculate_wrench();
+
+ // Calculate PWM signals from the control signal
+ msr::airlib::vector pwm = calculatePWM(wrench);
+
+ // Set the duration for which the PWM signals will be applied
+ const float duration = 0.01f;
+
+ // Send PWM signals to the ROV asynchronously
+ client.moveByMotorPWMsAsync(pwm, duration);
+
+ // Increment time for the next iteration
+ time += 0.01; // Adjust as needed based on the desired time step
}
- */
+
+
return 0;
}
diff --git a/HelloRov/rov_control.hpp b/HelloRov/rov_control.hpp
new file mode 100644
index 000000000..47e0901b1
--- /dev/null
+++ b/HelloRov/rov_control.hpp
@@ -0,0 +1,110 @@
+// Includes necessary headers
+#include
+#include
+#include
+#include // Include the cmath header for mathematical functions
+
+// Include external dependencies
+#include "vehicles/rov/api/RovRpcLibClient.hpp"
+#include "common/common_utils/FileSystem.hpp"
+#include "common/VectorMath.hpp"
+//using namespace msr::airlib;
+
+// Namespace declaration
+
+
+// Class definition for the ROV control
+class RovControl
+{
+public:
+ RovControl(msr::airlib::RovRpcLibClient& client, const Eigen::Vector4d& reference)
+ : client_(client), reference_(reference) {}
+
+ // Method to calculate wrench (control signal) based on PID control
+ Eigen::Vector4d calculate_wrench()
+ {
+ // Get current position from client
+ Eigen::VectorXd state = getState();
+
+ // Calculate error between reference and actual state
+ Eigen::VectorXd error(4);
+ error << reference_(0) - state(0), reference_(1) - state(1), reference_(2) - state(2), reference_(3) - state(6);
+
+ // Calculate control signal using PID
+ Eigen::Vector4d wrench;
+ wrench << Kp_ * error(0) + Kd_ * (error(0) - error_t_(0)) + Ki_ * error_i_(0),
+ Kp_ * error(1) + Kd_ * (error(1) - error_t_(1)) + Ki_ * error_i_(1),
+ Kp_ * error(2) + Kd_ * (error(2) - error_t_(2)) + Ki_ * error_i_(2),
+ (Kp_ * error(3) + Kd_ * (error(3) - error_t_(3)) + Ki_ * error_i_(3));
+
+
+ // Rotate the wrench from world frame to body frame by yaw
+ double yaw = state(6); // Assuming state(7) represents yaw angle
+ yaw = -yaw;
+ // Construct rotation matrix for 3D rotation about the z-axis (yaw)
+ Eigen::Matrix3d R;
+ R << cos(yaw), sin(yaw), 0,
+ -sin(yaw), cos(yaw), 0,
+ 0, 0, 1;
+
+ // Apply the rotation to the forces (first 3 elements of wrench)
+ Eigen::Vector3d forces_world(wrench.head<3>());
+ Eigen::Vector3d forces_body = R * forces_world;
+
+ // Update the first 3 elements of the wrench with the rotated forces
+ wrench.head<3>() = forces_body;
+
+ // Output debugging information
+ std::cout << std::endl
+ << "error_x " << error(0) << std::endl;
+ std::cout << std::endl
+ << "error_y " << error(1) << std::endl;
+ std::cout << std::endl
+ << "error_z " << error(2) << std::endl;
+ std::cout << std::endl
+ << "error_YAW " << error(3) << std::endl;
+
+ //error t-1
+ error_t_ = error;
+
+ //add integral error
+ error_i_ += error;
+ return wrench;
+ }
+
+ // Method to retrieve the current state (position and orientation) from the ROV
+ Eigen::VectorXd getState()
+ {
+ auto position = client_.getRovState().getPosition();
+
+ float pitch, roll, yaw;
+
+ // Convert quaternion orientation to Euler angles
+ msr::airlib::VectorMathf::toEulerianAngle(client_.getRovState().getOrientation(), pitch, roll, yaw);
+
+ // Construct state vector (position and yaw angle)
+ Eigen::VectorXd rov_state(8);
+ rov_state << position.x(),
+ position.y(),
+ -position.z(), // Negate z-axis position to match coordinate convention
+ 0.0, // Assuming velocity components are not used
+ 0.0,
+ 0.0,
+ -yaw, // Negative sign to match rotation convention
+ 0.0;
+
+ return rov_state;
+ }
+
+ Eigen::Vector4d error_t_;
+ Eigen::Vector4d error_i_;
+ Eigen::Vector4d reference_; // Reference vector for position and yaw
+
+private:
+ // Proportional gain for PID control
+ const double Kp_ = 20.0;
+ const double Kd_ = 1.0;
+ const double Ki_ = 0.0;
+ msr::airlib::RovRpcLibClient& client_; // Reference to ROV RPC client
+};
+
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
index fb3668bee..1a2ab19b8 100644
--- a/cmake/CMakeLists.txt
+++ b/cmake/CMakeLists.txt
@@ -6,7 +6,7 @@ add_subdirectory("AirLib")
add_subdirectory("MavLinkCom")
add_subdirectory("AirLibUnitTests")
add_subdirectory("HelloDrone")
-#add_subdirectory("HelloRov")
+add_subdirectory("HelloRov")
add_subdirectory("HelloSpawnedDrones")
add_subdirectory("HelloCar")
add_subdirectory("DroneShell")
diff --git a/cmake/HelloRov/CMakeLists.txt b/cmake/HelloRov/CMakeLists.txt
new file mode 100644
index 000000000..19c4322df
--- /dev/null
+++ b/cmake/HelloRov/CMakeLists.txt
@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.5.0)
+project(HelloRov)
+
+LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../cmake-modules")
+INCLUDE("${CMAKE_CURRENT_LIST_DIR}/../cmake-modules/CommonSetup.cmake")
+CommonSetup()
+
+IncludeEigen()
+
+SetupConsoleBuild()
+
+## Specify additional locations of header files
+include_directories(
+ ${AIRSIM_ROOT}/HelloRov
+ ${AIRSIM_ROOT}/AirLib/include
+ ${RPC_LIB_INCLUDES}
+ ${AIRSIM_ROOT}/MavLinkCom/include
+ ${AIRSIM_ROOT}/MavLinkCom/common_utils
+)
+
+AddExecutableSource()
+
+CommonTargetLink()
+target_link_libraries(${PROJECT_NAME} AirLib)
+target_link_libraries(${PROJECT_NAME} ${RPC_LIB})