Rosify

.gitignore

@@ -1,2 +1,5 @@
 build/*
 *.user
+.vscode/
+.DS_Store
+logs/

.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

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
+)

include/multirotor_sim/controller_ros.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "controller.h"
+#include "dynamics.h"
+
+#include <iostream>
+#include <fstream> //for logging
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <nav_msgs/Odometry.h>
+#include <rosflight_msgs/Command.h>
+
+// 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;
+}; 

package.xml

@@ -0,0 +1,18 @@
+<package format="2">
+  <name>multirotor_sim</name>
+  <version>0.0.1</version>
+  <description>
+  This is a tightly-coupled visual-inertial EKF which operates on the manifold
+  </description>
+  <maintainer email="[email protected]">jerelbn</maintainer>
+  <license>BSD-3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>roslib</depend>
+  <depend>rosflight_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>message_generation</depend>
+
+</package>

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()

src/controller_ros.cpp

@@ -0,0 +1,125 @@
+#include <controller_ros.h>
+#include <ros/package.h>
+
+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<rosflight_msgs::Command>("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<std::string>("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);
+}