diff --git a/.gitignore b/.gitignore index 3db7a99..a1b8138 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,5 @@ build/* *.user +.vscode/ +.DS_Store +logs/ diff --git a/.gitmodules b/.gitmodules index d88f7ad..eb42657 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "lib/geometry"] path = lib/geometry - url = git@github.com:superjax/geometry + url = https://github.com/superjax/geometry.git [submodule "lib/nanoflann_eigen"] path = lib/nanoflann_eigen - url = git@github.com:superjax/nanoflann_eigen.git + url = https://github.com/superjax/nanoflann_eigen.git diff --git a/CMakeLists.txt b/CMakeLists.txt index b4ffdfc..0a729ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,12 +1,25 @@ cmake_minimum_required (VERSION 2.8.11) project (multirotor_sim) +set(CMAKE_CXX_FLAGS "--std=c++17") set(CMAKE_POSITION_INDEPENDENT_CODE ON) +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + rosflight_msgs + nav_msgs +) + find_package(Boost REQUIRED COMPONENTS system thread) find_package(Eigen3 REQUIRED) find_package(yaml-cpp REQUIRED) +catkin_package ( + INCLUDE_DIRS include + CATKIN_DEPENDS roscpp nav_msgs rosflight_msgs roslib +) + if (NOT TARGET geometry) add_subdirectory(lib/geometry) include_directories(lib/geometry/include) @@ -31,4 +44,15 @@ add_library(multirotor_sim STATIC src/simulator.cpp src/environment.cpp ) -target_link_libraries(multirotor_sim ${YAML_CPP_LIBRARIES} stdc++fs geometry nanoflann_eigen) +target_link_libraries(multirotor_sim + ${YAML_CPP_LIBRARIES} + stdc++fs + geometry + nanoflann_eigen +) + +add_executable(multirotor_node src/controller_ros.cpp) +target_link_libraries(multirotor_node + ${catkin_LIBRARIES} + multirotor_sim +) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h new file mode 100644 index 0000000..ca8b10d --- /dev/null +++ b/include/multirotor_sim/controller_ros.h @@ -0,0 +1,48 @@ +#pragma once + +#include "controller.h" +#include "dynamics.h" + +#include +#include //for logging + +#include +#include +#include +#include + +// when defined, this will create binary dumps +// of the input and output of the controller +#define ROS_LOG_DUMP + +class Controller_ROS +{ +public: + Controller_ROS(); + ~Controller_ROS(); + void odometry_callback(const nav_msgs::OdometryConstPtr &msg); + +private: + // logging methods + void log_controller(const double t); + void init_log(std::string baseLogDir); + // describes the logs we're creating + // used as indicies into our array of output streams + typedef enum { + ODOM_IN, + COMMAND_OUT, + TOTAL_LOGS + } log_t; + std::ofstream logs_[TOTAL_LOGS]; + + ros::NodeHandle nh_; + ros::Subscriber odometry_sub_; + ros::Publisher command_pub_; + rosflight_msgs::Command command_msg_; + controller::Controller controller_; + dynamics::xVector parsed_odom_; + dynamics::commandVector command_out_; + ros::Time start_time_; + + bool odom_init_ = false; +}; \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a33073a --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + multirotor_sim + 0.0.1 + + This is a tightly-coupled visual-inertial EKF which operates on the manifold + + jerelbn + BSD-3 + + catkin + + roscpp + roslib + rosflight_msgs + nav_msgs + message_generation + + \ No newline at end of file diff --git a/scripts/plot_logs.py b/scripts/plot_logs.py new file mode 100644 index 0000000..aa0bb41 --- /dev/null +++ b/scripts/plot_logs.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python + +import os +import numpy as np +import matplotlib.pyplot as plt + +def main(): + ODOM_VAL_PER_ROW = 14 + COMMAND_VAL_PER_ROW = 5 + + logDir = os.path.dirname(os.path.realpath(__file__)) + '/../logs/' + print("looking in {} for logs".format(logDir)) + + # get data and shape it + rawOdom = np.fromfile(logDir + 'odom.bin', float, -1, "") + rawOdom = np.reshape(rawOdom, (-1, ODOM_VAL_PER_ROW)) + + rawCommand = np.fromfile(logDir + 'command.bin', float, -1, "") + rawCommand = np.reshape(rawCommand, (-1, COMMAND_VAL_PER_ROW)) + + # Command column format: time, thrust, x, y, z + commandYLabels = ['Command: Thrust', 'Command: X', 'Command: Y', 'Command: Z'] + colors = ['b-', 'r-', 'y-', 'g-'] + + plt.figure('Command Out') + for i in range(1,COMMAND_VAL_PER_ROW): + plt.subplot(2,2,i) + plt.plot(rawCommand[:,0], rawCommand[:,i], colors[i-1]) + plt.ylabel(commandYLabels[i-1]) + + genericLabels = ['W', 'X', 'Y', 'Z'] + + plt.figure("Estimator Pose In") + labelBase = 'Estimator: ' + for i in range(1, 4): + plt.subplot(3,1,i) + plt.plot(rawOdom[:,0], rawOdom[:,i], colors[i]) + plt.ylabel(labelBase + genericLabels[i]) + + plt.figure('Estimator Vel In') + labelBase = 'Estimator Vel: ' + indexBase = 3 + for i in range(1,4): + plt.subplot(3,1,i) + plt.plot(rawOdom[:,0], rawOdom[:,(indexBase + i)], colors[i]) + plt.ylabel(labelBase + genericLabels[i]) + + plt.figure('Estimator Orientation In') + labelBase = 'Estimator Orientation: ' + indexBase = 6 + for i in range(1,5): + plt.subplot(2,2,i) + plt.plot(rawOdom[:,0], rawOdom[:,(indexBase + i)], colors[i-1]) + plt.ylabel(labelBase + genericLabels[i-1]) + + plt.figure('Estimator Angular In') + labelBase = 'Estimator Angular: ' + indexBase = 10 + for i in range(1,4): + plt.subplot(3,1,i) + plt.plot(rawOdom[:,0], rawOdom[:,(indexBase + i)], colors[i]) + plt.ylabel(labelBase + genericLabels[i]) + + plt.show() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp new file mode 100644 index 0000000..a7bce09 --- /dev/null +++ b/src/controller_ros.cpp @@ -0,0 +1,125 @@ +#include +#include + +using namespace std; + +Controller_ROS::Controller_ROS() +{ + controller_ = controller::Controller(); + odometry_sub_ = nh_.subscribe("odom", 100, &Controller_ROS::odometry_callback, this); + command_pub_ = nh_.advertise("command", 1); + + ros::NodeHandle nh_private("~"); + string param_file = "/home/len0rd/catkin_ws/src/multirotor_sim/params/sim_params.yaml"; + ROS_WARN_COND(!nh_private.getParam("param_file", param_file), "Didn't specify `param_file` parameter! Using default location..."); + + controller_.load(param_file); //todo: change to ros params? + + #ifdef ROS_LOG_DUMP + std::string defaultLogDir = ros::package::getPath("multirotor_sim") + "/logs"; //+ to_string(ros::Time::now().sec); + //get or use default log dir + string logDir; + nh_private.param("log_directory", logDir, defaultLogDir); + init_log(logDir); + #endif +} + +Controller_ROS::~Controller_ROS() +{ + #ifdef ROS_LOG_DUMP + // close out our loggers + for (int i = 0; i < TOTAL_LOGS; i++) + { + logs_[i] << endl; + logs_[i].close(); + } + #endif +} + +void Controller_ROS::log_controller(const double t) +{ + // logs the current input and output to the controller + + // log the input odometry from estimator + logs_[ODOM_IN].write((char*)&t, sizeof(double)); + logs_[ODOM_IN].write((char*)parsed_odom_.data(), sizeof(double) * parsed_odom_.rows()); + + // log the output command from the controller + logs_[COMMAND_OUT].write((char*)&t, sizeof(double)); + logs_[COMMAND_OUT].write((char*)command_out_.data(), sizeof(double) * command_out_.rows()); +} + +void Controller_ROS::init_log(string baseLogDir) +{ + // initialize the logs variable for each log file + + // make the log directory if it doesn't already exist + system(("mkdir -p " + baseLogDir).c_str()); + + // initialize the log files + logs_[ODOM_IN].open(baseLogDir + "/odom.bin", std::ofstream::out | std::ofstream::trunc); + logs_[COMMAND_OUT].open(baseLogDir + "/command.bin", std::ofstream::out | std::ofstream::trunc); +} + +void Controller_ROS::odometry_callback(const nav_msgs::OdometryConstPtr &msg) +{ + // subscribes to the 'Odom' topic of the estimator. The odometry message + // here is the output of the estimator + if (!odom_init_) + { + //this is the first time callback was run, get the start time + odom_init_ = true; + start_time_ = msg->header.stamp; + return; + } + + // parse message + parsed_odom_(dynamics::PX, 0) = msg->pose.pose.position.x; + parsed_odom_(dynamics::PY, 0) = msg->pose.pose.position.y; + parsed_odom_(dynamics::PZ, 0) = msg->pose.pose.position.z; + + parsed_odom_(dynamics::VX, 0) = msg->twist.twist.linear.x; + parsed_odom_(dynamics::VY, 0) = msg->twist.twist.linear.y; + parsed_odom_(dynamics::VZ, 0) = msg->twist.twist.linear.z; + + parsed_odom_(dynamics::QW, 0) = msg->pose.pose.orientation.w; + parsed_odom_(dynamics::QX, 0) = msg->pose.pose.orientation.x; + parsed_odom_(dynamics::QY, 0) = msg->pose.pose.orientation.y; + parsed_odom_(dynamics::QZ, 0) = msg->pose.pose.orientation.z; + + parsed_odom_(dynamics::WX, 0) = msg->twist.twist.angular.x; + parsed_odom_(dynamics::WY, 0) = msg->twist.twist.angular.y; + parsed_odom_(dynamics::WZ, 0) = msg->twist.twist.angular.z; + + ros::Time ros_ts = msg->header.stamp; + const double t = (ros_ts - start_time_).toSec(); + + //compute control + controller_.computeControl(parsed_odom_, t, command_out_); + + // publish resulting command + command_msg_.header.stamp = ros_ts; + command_msg_.mode = rosflight_msgs::Command::MODE_ROLL_PITCH_YAWRATE_THROTTLE; + command_msg_.ignore = rosflight_msgs::Command::IGNORE_NONE; + command_msg_.x = command_out_(dynamics::TAUX); + command_msg_.y = command_out_(dynamics::TAUY); + command_msg_.z = command_out_(dynamics::TAUZ); + command_msg_.F = command_out_(dynamics::THRUST); + + command_pub_.publish(command_msg_); + + // log all this out if we're supposed to: + #ifdef ROS_LOG_DUMP + log_controller(t); + #endif +} + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "multirotor_controller_node"); + Controller_ROS controller_ros; + + ros::spin(); + + return(0); +} \ No newline at end of file