From 1fd2b233bffdb22e9af4979274b5d32f90cb68ff Mon Sep 17 00:00:00 2001 From: len0rd Date: Thu, 1 Nov 2018 15:48:50 -0600 Subject: [PATCH 01/14] https submodules, vs ssh --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From f734f697f991de897d2cd99a52470e66e005cf28 Mon Sep 17 00:00:00 2001 From: len0rd Date: Thu, 1 Nov 2018 16:37:47 -0600 Subject: [PATCH 02/14] start on ros wrapper --- include/multirotor_sim/controller_ros.h | 5 +++++ src/controller_ros.cpp | 23 +++++++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 include/multirotor_sim/controller_ros.h create mode 100644 src/controller_ros.cpp diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h new file mode 100644 index 0000000..7a87a6d --- /dev/null +++ b/include/multirotor_sim/controller_ros.h @@ -0,0 +1,5 @@ +#include "controller.h" + +#include +#include +#include \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp new file mode 100644 index 0000000..c7bc659 --- /dev/null +++ b/src/controller_ros.cpp @@ -0,0 +1,23 @@ +#include + + + + +void odometry_callback(const nav_msgs::Odometry& msg){ + +} + + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "multirotor_controller_node"); + + ros::NodeHandle nh; + + ros::Subscriber odometry_sub_ = nh.subscribe("nav_msgs/Odometry", 1, &odometry_callback, this); + ros::Publisher command_pub_ = nh.advertise() + + ros::spin(); + + return(0); +} \ No newline at end of file From 849b05ead040f8cbc4254c3c2751dff39c13fdbf Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 2 Nov 2018 13:36:39 -0600 Subject: [PATCH 03/14] add catkin stuff, controller_ros class --- CMakeLists.txt | 11 +++++++++++ include/multirotor_sim/controller_ros.h | 15 ++++++++++++++- package.xml | 17 +++++++++++++++++ src/controller_ros.cpp | 20 ++++++++++++-------- 4 files changed, 54 insertions(+), 9 deletions(-) create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index b4ffdfc..dc772a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,10 +3,21 @@ project (multirotor_sim) set(CMAKE_POSITION_INDEPENDENT_CODE ON) +find_package(catkin REQUIRED COMPONENTS + roscpp + 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 +) + if (NOT TARGET geometry) add_subdirectory(lib/geometry) include_directories(lib/geometry/include) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index 7a87a6d..4c78b1f 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -2,4 +2,17 @@ #include #include -#include \ No newline at end of file +#include + +class Controller_ROS +{ +public: + Controller_ROS(); + void odometry_callback(const nav_msgs::Odometry& msg); + +private: + ros::NodeHandle nh_; + ros::Subscriber odometry_sub_; + ros::Publisher command_pub_; + controller::Controller controller_; +}; \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4857cb3 --- /dev/null +++ b/package.xml @@ -0,0 +1,17 @@ + + multirotor_controller + 0.1 + + This is a tightly-coupled visual-inertial EKF which operates on the manifold + + jerelbn + BSD-3 + + catkin + + roscpp + rosflight_msgs + nav_msgs + message_generation + + \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index c7bc659..6fcb701 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -1,21 +1,25 @@ #include - - -void odometry_callback(const nav_msgs::Odometry& msg){ - +Controller_ROS::Controller_ROS() : + nh_("~") +{ + controller_ = controller::Controller(); + controller.load("~/catkin_ws/src/multirotor_sim/params/sim_params.yaml"); //todo: change to ros params? + odometry_sub_ = nh.subscribe("nav_msgs/Odometry", 100, &odometry_callback, this); + command_pub_ = nh.advertise("command", 100); } +Controller_ROS::odometry_callback(const nav_msgs::Odometry& msg) +{ + std::cout << "odom callback" << endl; +} int main(int argc, char* argv[]) { ros::init(argc, argv, "multirotor_controller_node"); - ros::NodeHandle nh; - - ros::Subscriber odometry_sub_ = nh.subscribe("nav_msgs/Odometry", 1, &odometry_callback, this); - ros::Publisher command_pub_ = nh.advertise() + Controller_ROS controller_ros; ros::spin(); From ce6a1c23153d09eca6ecfb29bb887fcf5e6cfe47 Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 2 Nov 2018 14:12:24 -0600 Subject: [PATCH 04/14] fix build errors --- .gitignore | 1 + CMakeLists.txt | 14 +++++++++++++- include/multirotor_sim/controller_ros.h | 1 + package.xml | 6 +++--- src/controller_ros.cpp | 10 +++++----- 5 files changed, 23 insertions(+), 9 deletions(-) diff --git a/.gitignore b/.gitignore index 3db7a99..e42379f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build/* *.user +.vscode/ diff --git a/CMakeLists.txt b/CMakeLists.txt index dc772a6..b7a9f72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ 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 @@ -42,4 +43,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 index 4c78b1f..c3ab62c 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -3,6 +3,7 @@ #include #include #include +#include class Controller_ROS { diff --git a/package.xml b/package.xml index 4857cb3..e13e71b 100644 --- a/package.xml +++ b/package.xml @@ -1,10 +1,10 @@ - multirotor_controller - 0.1 + multirotor_sim + 0.0.1 This is a tightly-coupled visual-inertial EKF which operates on the manifold - jerelbn + jerelbn BSD-3 catkin diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index 6fcb701..a228d36 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -5,14 +5,14 @@ Controller_ROS::Controller_ROS() : nh_("~") { controller_ = controller::Controller(); - controller.load("~/catkin_ws/src/multirotor_sim/params/sim_params.yaml"); //todo: change to ros params? - odometry_sub_ = nh.subscribe("nav_msgs/Odometry", 100, &odometry_callback, this); - command_pub_ = nh.advertise("command", 100); + controller_.load("~/catkin_ws/src/multirotor_sim/params/sim_params.yaml"); //todo: change to ros params? + odometry_sub_ = nh_.subscribe("nav_msgs/Odometry", 100, &Controller_ROS::odometry_callback, this); + command_pub_ = nh_.advertise("command", 1); } -Controller_ROS::odometry_callback(const nav_msgs::Odometry& msg) +void Controller_ROS::odometry_callback(const nav_msgs::Odometry& msg) { - std::cout << "odom callback" << endl; + std::cout << "odom callback" << std::endl; } int main(int argc, char* argv[]) From 5a5e81b73422329c1e119262d189d2580914f988 Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 2 Nov 2018 14:52:05 -0600 Subject: [PATCH 05/14] can now pass a paramater for the parameter file location... --- include/multirotor_sim/controller_ros.h | 2 ++ src/controller_ros.cpp | 10 ++++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index c3ab62c..37a42b0 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -1,5 +1,7 @@ #include "controller.h" +#include + #include #include #include diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index a228d36..229a5a0 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -1,13 +1,19 @@ #include +using namespace std; Controller_ROS::Controller_ROS() : nh_("~") { controller_ = controller::Controller(); - controller_.load("~/catkin_ws/src/multirotor_sim/params/sim_params.yaml"); //todo: change to ros params? - odometry_sub_ = nh_.subscribe("nav_msgs/Odometry", 100, &Controller_ROS::odometry_callback, this); + odometry_sub_ = nh_.subscribe("vi_ekf_node/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? } void Controller_ROS::odometry_callback(const nav_msgs::Odometry& msg) From 08b4789290cc48be8e82b89f7834a84cadbb5aeb Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 2 Nov 2018 16:49:59 -0600 Subject: [PATCH 06/14] fleshing out odom controller --- include/multirotor_sim/controller_ros.h | 5 +++- src/controller_ros.cpp | 36 ++++++++++++++++++++----- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index 37a42b0..9d0ced0 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -1,4 +1,5 @@ #include "controller.h" +#include "dynamics.h" #include @@ -18,4 +19,6 @@ class Controller_ROS ros::Subscriber odometry_sub_; ros::Publisher command_pub_; controller::Controller controller_; -}; \ No newline at end of file + dynamics::xVector parsed_odom_; + dynamics::commandVector command_out; +}; \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index 229a5a0..01afd85 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -3,10 +3,9 @@ using namespace std; Controller_ROS::Controller_ROS() : - nh_("~") { controller_ = controller::Controller(); - odometry_sub_ = nh_.subscribe("vi_ekf_node/odom", 100, &Controller_ROS::odometry_callback, this); + odometry_sub_ = nh_.subscribe("odom", 100, &Controller_ROS::odometry_callback, this); command_pub_ = nh_.advertise("command", 1); ros::NodeHandle nh_private("~"); @@ -16,15 +15,40 @@ Controller_ROS::Controller_ROS() : controller_.load(param_file); //todo: change to ros params? } -void Controller_ROS::odometry_callback(const nav_msgs::Odometry& msg) -{ - std::cout << "odom callback" << std::endl; +void Controller_ROS::odometry_callback(const nav_msgs::Odometry &msg) +{ + // parse message + //todo: TYLER -> why is it msg. instead of msg-> ?? + 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; + + //figure out time stuff: + + + //compute control + + // computeControl(parsed_odom_, TIME_STAMPPPPP, command_out); + + // publish resulting command } int main(int argc, char* argv[]) { ros::init(argc, argv, "multirotor_controller_node"); - Controller_ROS controller_ros; ros::spin(); From 98be7ceaa810a39b2b586591065f34d73a043af1 Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 2 Nov 2018 16:52:08 -0600 Subject: [PATCH 07/14] typo --- src/controller_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index 01afd85..8c66e06 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -2,7 +2,7 @@ using namespace std; -Controller_ROS::Controller_ROS() : +Controller_ROS::Controller_ROS() { controller_ = controller::Controller(); odometry_sub_ = nh_.subscribe("odom", 100, &Controller_ROS::odometry_callback, this); From d17142ac9a6378fd907920f96867dbaff801a68a Mon Sep 17 00:00:00 2001 From: len0rd Date: Mon, 5 Nov 2018 14:30:10 -0700 Subject: [PATCH 08/14] finished callback. guarantee bugs are enclosed --- include/multirotor_sim/controller_ros.h | 5 ++++- src/controller_ros.cpp | 24 ++++++++++++++++++++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index 9d0ced0..a4aa5a2 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -18,7 +18,10 @@ class Controller_ROS 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; + dynamics::commandVector command_out_; + ros::Time start_time_; + bool odom_init_ = false; }; \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index 8c66e06..c216f7c 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -17,6 +17,14 @@ Controller_ROS::Controller_ROS() void Controller_ROS::odometry_callback(const nav_msgs::Odometry &msg) { + 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 //todo: TYLER -> why is it msg. instead of msg-> ?? parsed_odom_(dynamics::PX, 0) = msg.pose.pose.position.x; @@ -36,14 +44,22 @@ void Controller_ROS::odometry_callback(const nav_msgs::Odometry &msg) parsed_odom_(dynamics::WY, 0) = msg.twist.twist.angular.y; parsed_odom_(dynamics::WZ, 0) = msg.twist.twist.angular.z; - //figure out time stuff: - + ros::Time ros_ts = msg.header.stamp; + const double t = (ros_ts - start_time_).toSec() //compute control - - // computeControl(parsed_odom_, TIME_STAMPPPPP, command_out); + controller_.computeControl(parsed_odom_, t, command_out_); // publish resulting command + command_msg_.header.stamp = ros_ts; + command_msg_.mode = rosflight_msgs::MODE_ROLL_PITCH_YAWRATE_THROTTLE; + command_msg_.ignore = rosflight_msgs::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_); } int main(int argc, char* argv[]) From ba7c693593baa8be59151bbbec06d8ac156531d3 Mon Sep 17 00:00:00 2001 From: len0rd Date: Mon, 5 Nov 2018 14:35:53 -0700 Subject: [PATCH 09/14] bug fix --- src/controller_ros.cpp | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index c216f7c..84670e2 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -15,45 +15,45 @@ Controller_ROS::Controller_ROS() controller_.load(param_file); //todo: change to ros params? } -void Controller_ROS::odometry_callback(const nav_msgs::Odometry &msg) +void Controller_ROS::odometry_callback(const nav_msgs::OdometryConstPtr &msg) { if (!odom_init_) { //this is the first time callback was run, get the start time odom_init_ = true; - start_time_ = msg.header.stamp; + start_time_ = msg->header.stamp; return; } // parse message //todo: TYLER -> why is it msg. instead of msg-> ?? - 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::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::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::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; + 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() + 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::MODE_ROLL_PITCH_YAWRATE_THROTTLE; - command_msg_.ignore = rosflight_msgs::IGNORE_NONE; + 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); From d772966b2d8aebeab35a8497485f3c43389236ab Mon Sep 17 00:00:00 2001 From: len0rd Date: Mon, 5 Nov 2018 14:39:12 -0700 Subject: [PATCH 10/14] working --- include/multirotor_sim/controller_ros.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index a4aa5a2..f6271a8 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -12,7 +12,7 @@ class Controller_ROS { public: Controller_ROS(); - void odometry_callback(const nav_msgs::Odometry& msg); + void odometry_callback(const nav_msgs::OdometryConstPtr &msg); private: ros::NodeHandle nh_; From 6552ba00b8c422b273e98da95aaf7d6ceb96c065 Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 9 Nov 2018 14:59:48 -0700 Subject: [PATCH 11/14] logging builds --- CMakeLists.txt | 3 +- include/multirotor_sim/controller_ros.h | 21 ++++++++++ package.xml | 1 + src/controller_ros.cpp | 54 ++++++++++++++++++++++++- 4 files changed, 77 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b7a9f72..0a729ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) find_package(catkin REQUIRED COMPONENTS roscpp + roslib rosflight_msgs nav_msgs ) @@ -16,7 +17,7 @@ find_package(yaml-cpp REQUIRED) catkin_package ( INCLUDE_DIRS include - CATKIN_DEPENDS roscpp nav_msgs rosflight_msgs + CATKIN_DEPENDS roscpp nav_msgs rosflight_msgs roslib ) if (NOT TARGET geometry) diff --git a/include/multirotor_sim/controller_ros.h b/include/multirotor_sim/controller_ros.h index f6271a8..ca8b10d 100644 --- a/include/multirotor_sim/controller_ros.h +++ b/include/multirotor_sim/controller_ros.h @@ -1,20 +1,40 @@ +#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_; @@ -23,5 +43,6 @@ class Controller_ROS 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 index e13e71b..a33073a 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,7 @@ catkin roscpp + roslib rosflight_msgs nav_msgs message_generation diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index 84670e2..e37e9c2 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -1,4 +1,5 @@ #include +#include using namespace std; @@ -13,10 +14,57 @@ Controller_ROS::Controller_ROS() 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) +{ + // logs_ = ofstream[TOTAL_LOGS]; + // 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 @@ -26,7 +74,6 @@ void Controller_ROS::odometry_callback(const nav_msgs::OdometryConstPtr &msg) } // parse message - //todo: TYLER -> why is it msg. instead of msg-> ?? 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; @@ -60,6 +107,11 @@ void Controller_ROS::odometry_callback(const nav_msgs::OdometryConstPtr &msg) 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[]) From b9c32c5a4a26def97d6d6989e2f0c1ae158da43e Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 9 Nov 2018 16:37:23 -0700 Subject: [PATCH 12/14] log dumps working from controller_ros. started work on the python plotter --- .gitignore | 2 ++ scripts/plot_logs.py | 26 ++++++++++++++++++++++++++ src/controller_ros.cpp | 4 ++-- 3 files changed, 30 insertions(+), 2 deletions(-) create mode 100644 scripts/plot_logs.py diff --git a/.gitignore b/.gitignore index e42379f..a1b8138 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ build/* *.user .vscode/ +.DS_Store +logs/ diff --git a/scripts/plot_logs.py b/scripts/plot_logs.py new file mode 100644 index 0000000..30135a9 --- /dev/null +++ b/scripts/plot_logs.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python + +import os +import numpy as np +# from tqdm import tqdm # progress bar +# 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)) + + # Formatted Time, Thrust, X, Y, Z for clmns + print(rawCommand[0:4,:]) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/controller_ros.cpp b/src/controller_ros.cpp index e37e9c2..a7bce09 100644 --- a/src/controller_ros.cpp +++ b/src/controller_ros.cpp @@ -47,12 +47,12 @@ void Controller_ROS::log_controller(const double t) // 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) { - // logs_ = ofstream[TOTAL_LOGS]; + // initialize the logs variable for each log file + // make the log directory if it doesn't already exist system(("mkdir -p " + baseLogDir).c_str()); From aa15a0710f774fccad84808eb4fcd362eee4726c Mon Sep 17 00:00:00 2001 From: len0rd Date: Wed, 14 Nov 2018 17:06:04 -0700 Subject: [PATCH 13/14] basic plots --- scripts/plot_logs.py | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/scripts/plot_logs.py b/scripts/plot_logs.py index 30135a9..34e886d 100644 --- a/scripts/plot_logs.py +++ b/scripts/plot_logs.py @@ -2,8 +2,7 @@ import os import numpy as np -# from tqdm import tqdm # progress bar -# import matplotlib.pyplot as plt +import matplotlib.pyplot as plt def main(): ODOM_VAL_PER_ROW = 14 @@ -19,8 +18,26 @@ def main(): rawCommand = np.fromfile(logDir + 'command.bin', float, -1, "") rawCommand = np.reshape(rawCommand, (-1, COMMAND_VAL_PER_ROW)) - # Formatted Time, Thrust, X, Y, Z for clmns - print(rawCommand[0:4,:]) + # 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]) + + + odomPoseLabels = ['Estimator: X', 'Estimator: Y', 'Estimator: Z'] + plt.figure("Estimator Pose In") + for i in range(1, 4): + plt.subplot(3,1,i) + plt.plot(rawOdom[:,0], rawOdom[:,i], colors[i-1]) + plt.ylabel(odomPoseLabels[i-1]) + + plt.show() + if __name__ == '__main__': main() \ No newline at end of file From 68adc087d57fba4f1975ff4f12c981143e8c324b Mon Sep 17 00:00:00 2001 From: len0rd Date: Thu, 15 Nov 2018 16:40:19 -0700 Subject: [PATCH 14/14] plotting everything. a bit unruly with 5 seperate windows --- scripts/plot_logs.py | 32 ++++++++++++++++++++++++++++---- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/scripts/plot_logs.py b/scripts/plot_logs.py index 34e886d..aa0bb41 100644 --- a/scripts/plot_logs.py +++ b/scripts/plot_logs.py @@ -28,16 +28,40 @@ def main(): plt.plot(rawCommand[:,0], rawCommand[:,i], colors[i-1]) plt.ylabel(commandYLabels[i-1]) + genericLabels = ['W', 'X', 'Y', 'Z'] - odomPoseLabels = ['Estimator: X', 'Estimator: Y', 'Estimator: 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-1]) - plt.ylabel(odomPoseLabels[i-1]) + plt.plot(rawOdom[:,0], rawOdom[:,i], colors[i]) + plt.ylabel(labelBase + genericLabels[i]) - plt.show() + 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