From cc30c147285133a87c62d3d39eb4cccb07b94ea3 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 18:18:28 -0400 Subject: [PATCH 1/5] kr_mav_controllers ROS2 humble compatible --- kr_mav_controllers/CMakeLists.txt | 113 ++- kr_mav_controllers/README.md | 14 + kr_mav_controllers/cfg/SO3.cfg | 51 -- .../include/kr_mav_controllers/PIDControl.hpp | 8 +- .../{SO3Control.h => SO3Control.hpp} | 8 +- .../kr_mav_controllers/so3_control_tester.hpp | 100 +-- kr_mav_controllers/nodelet_plugin.xml | 23 - kr_mav_controllers/package.xml | 25 +- kr_mav_controllers/src/PIDControl.cpp | 3 +- kr_mav_controllers/src/SO3Control.cpp | 9 +- .../src/pid_control_component.cpp | 184 +++++ .../src/pid_control_nodelet.cpp | 173 ----- .../src/so3_control_component.cpp | 649 +++++++++++++++++ .../src/so3_control_nodelet.cpp | 376 ---------- kr_mav_controllers/src/so3_trpy_control.cpp | 660 ++++++++++++------ .../launch/so3_control_component_test.test.py | 91 +++ .../test/so3_control_component_test.cpp | 241 +++++++ .../test/so3_control_nodelet.test | 12 - .../test/so3_control_nodelet_test.cpp | 230 ------ 19 files changed, 1755 insertions(+), 1215 deletions(-) create mode 100644 kr_mav_controllers/README.md delete mode 100755 kr_mav_controllers/cfg/SO3.cfg rename kr_mav_controllers/include/kr_mav_controllers/{SO3Control.h => SO3Control.hpp} (93%) delete mode 100644 kr_mav_controllers/nodelet_plugin.xml create mode 100644 kr_mav_controllers/src/pid_control_component.cpp delete mode 100644 kr_mav_controllers/src/pid_control_nodelet.cpp create mode 100644 kr_mav_controllers/src/so3_control_component.cpp delete mode 100644 kr_mav_controllers/src/so3_control_nodelet.cpp create mode 100755 kr_mav_controllers/test/launch/so3_control_component_test.test.py create mode 100644 kr_mav_controllers/test/so3_control_component_test.cpp delete mode 100644 kr_mav_controllers/test/so3_control_nodelet.test delete mode 100644 kr_mav_controllers/test/so3_control_nodelet_test.cpp diff --git a/kr_mav_controllers/CMakeLists.txt b/kr_mav_controllers/CMakeLists.txt index 0a150fbb..7e105f62 100644 --- a/kr_mav_controllers/CMakeLists.txt +++ b/kr_mav_controllers/CMakeLists.txt @@ -1,83 +1,58 @@ cmake_minimum_required(VERSION 3.10) project(kr_mav_controllers) -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - geometry_msgs - kr_mav_msgs - nav_msgs - nodelet - roscpp - std_msgs - tf -) -find_package(Eigen3 REQUIRED NO_MODULE) - -generate_dynamic_reconfigure_options(cfg/SO3.cfg) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - CATKIN_DEPENDS - dynamic_reconfigure - geometry_msgs - kr_mav_msgs - nav_msgs - nodelet - roscpp - std_msgs - tf - DEPENDS - EIGEN3 -) - -add_library(kr_mav_so3_control src/SO3Control.cpp src/so3_control_nodelet.cpp src/so3_trpy_control.cpp) -target_include_directories(kr_mav_so3_control PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries(kr_mav_so3_control PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) -add_dependencies(kr_mav_so3_control ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -# TODO: PID control has not been updated for new messages add_library(kr_mav_pid_control src/PIDControl.cpp -# src/pid_control_nodelet.cpp) add_dependencies(kr_mav_pid_control ${catkin_EXPORTED_TARGETS}) -# target_link_libraries(kr_mav_pid_control ${catkin_LIBRARIES}) - -install( - TARGETS kr_mav_so3_control - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +include_directories(include) + +add_library(kr_mav_pid_control SHARED src/pid_control_component.cpp src/PIDControl.cpp) +ament_target_dependencies(kr_mav_pid_control rclcpp rclcpp_components kr_mav_msgs nav_msgs std_msgs tf2 tf2_geometry_msgs) +target_link_libraries(kr_mav_pid_control Eigen3::Eigen) +rclcpp_components_register_nodes(kr_mav_pid_control "PIDControlComponent") + +add_library(kr_mav_so3_control SHARED src/so3_control_component.cpp src/SO3Control.cpp src/so3_trpy_control.cpp) +ament_target_dependencies(kr_mav_so3_control rclcpp rclcpp_components kr_mav_msgs nav_msgs std_msgs geometry_msgs tf2 tf2_geometry_msgs) +target_link_libraries(kr_mav_so3_control Eigen3::Eigen) +rclcpp_components_register_nodes(kr_mav_so3_control "SO3ControlComponent") +rclcpp_components_register_nodes(kr_mav_so3_control "SO3TRPYControlComponent") + +install(TARGETS + kr_mav_pid_control + kr_mav_so3_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -# install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES nodelet_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) -if(CATKIN_ENABLE_TESTING) - find_package(catkin REQUIRED COMPONENTS - roscpp + include_directories(include) + ament_add_gtest_executable(so3_control_component_test test/so3_control_component_test.cpp) + ament_target_dependencies(so3_control_component_test std_msgs - nav_msgs kr_mav_msgs - rostest + nav_msgs + rclcpp ) - include_directories(include ${catkin_INCLUDE_DIRS}) - include_directories(${GTEST_INCLUDE_DIRS}) - - add_executable(so3_control_nodelet_test test/so3_control_nodelet_test.cpp) - # add_rostest_gtest(so3_control_nodelet_test test/so3_control_nodelet.test test/so3_control_nodelet_test.cpp) - target_link_libraries(so3_control_nodelet_test ${catkin_LIBRARIES} ${GTEST_LIBRARIES}) - - add_rostest(test/so3_control_nodelet.test) + add_launch_test(test/launch/so3_control_component_test.test.py TIMEOUT 80 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() + +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/kr_mav_controllers/README.md b/kr_mav_controllers/README.md new file mode 100644 index 00000000..04026d75 --- /dev/null +++ b/kr_mav_controllers/README.md @@ -0,0 +1,14 @@ +kr_mav_controllers +============= + +Package for SO3 Controllers. + +## Build Package +``` +colcon build --packages-select kr_mav_controllers +``` + +## Run Tests +``` +colcon test --packages-select kr_mav_controllers && colcon test-result --all --verbose +``` diff --git a/kr_mav_controllers/cfg/SO3.cfg b/kr_mav_controllers/cfg/SO3.cfg deleted file mode 100755 index 914be01d..00000000 --- a/kr_mav_controllers/cfg/SO3.cfg +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "kr_mav_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -tg = gen.add_group("Translation Gains") -# World Position Gains -tg.add("kp_x", double_t, 1, "World x position gain", 7.4, 0.0, 20.0) -tg.add("kp_y", double_t, 1, "World y position gain", 7.4, 0.0, 20.0) -tg.add("kp_z", double_t, 1, "World z position gain", 10.4, 0.0, 20.0) -# World Derivative Gai -tg.add("kd_x", double_t, 1, "World x derivative gain", 4.8, 0.0, 20.0) -tg.add("kd_y", double_t, 1, "World y derivative gain", 4.8, 0.0, 20.0) -tg.add("kd_z", double_t, 1, "World z derivative gain", 6.0, 0.0, 20.0) - -ig = gen.add_group("Integral Gains") -# World Integral Gains -ig.add("ki_x", double_t, 2, "World x integral gain", 0.0, 0.0, 0.2) -ig.add("ki_y", double_t, 2, "World y integral gain", 0.0, 0.0, 0.2) -ig.add("ki_z", double_t, 2, "World z integral gain", 0.0, 0.0, 0.2) -# Body Integral Gains -ig.add("kib_x", double_t, 2, "Body x integral gain", 0.0, 0.0, 0.2) -ig.add("kib_y", double_t, 2, "Body y integral gain", 0.0, 0.0, 0.2) -ig.add("kib_z", double_t, 2, "Body z integral gain", 0.0, 0.0, 0.2) - -ag = gen.add_group("Attitude Gains") -# Rotation Gains -ag.add("rot_x", double_t, 4, "Rotation x gain", 1.5, 0.0, 3.00) -ag.add("rot_y", double_t, 4, "Rotation y gain", 1.5, 0.0, 3.00) -ag.add("rot_z", double_t, 4, "Rotation z gain", 1.0, 0.0, 3.00) -# Angular Gains -ag.add("ang_x", double_t, 4, "Angular x gain", 0.13, 0.0, 1.00) -ag.add("ang_y", double_t, 4, "Angular y gain", 0.13, 0.0, 1.00) -ag.add("ang_z", double_t, 4, "Angular z gain", 0.10, 0.0, 1.00) - -# Low level corrections -corr = gen.add_group("Corrections") -# TODO: Determine limits -corr.add("kf_correction", double_t, 8, "kf", 0.0, 0.0, 0.0) -corr.add("roll_correction", double_t, 8, "roll", 0.0, 0.0, 0.0) -corr.add("pitch_correction", double_t, 8, "pitch", 0.0, 0.0, 0.0) - -# Misc -misc = gen.add_group("Maxes and limits") -misc.add("max_pos_int", double_t, 16, "World max integral", 0.0, 0.0, 4.00) -misc.add("max_pos_int_b", double_t, 16, "Body max integral", 0.0, 0.0, 4.00) -misc.add("max_tilt_angle", double_t, 16, "Max tilt angle", 3.14, 0.0, 3.14) - -exit(gen.generate(PACKAGE, "kr_mav_controllers", "SO3")) diff --git a/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp b/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp index a6be01f2..9d5f3e77 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/PIDControl.hpp @@ -1,5 +1,5 @@ -#ifndef PID_CONTROL_H -#define PID_CONTROL_H +#ifndef PID_CONTROL_HPP +#define PID_CONTROL_HPP #include @@ -22,7 +22,7 @@ class PIDControl const Eigen::Vector4f &getControls(void); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: // Inputs for the controller @@ -39,4 +39,4 @@ class PIDControl Eigen::Vector4f trpy_; }; -#endif +#endif // PID_CONTROL_HPP diff --git a/kr_mav_controllers/include/kr_mav_controllers/SO3Control.h b/kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp similarity index 93% rename from kr_mav_controllers/include/kr_mav_controllers/SO3Control.h rename to kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp index a30350c1..91b6bc8b 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/SO3Control.h +++ b/kr_mav_controllers/include/kr_mav_controllers/SO3Control.hpp @@ -1,5 +1,5 @@ -#ifndef SO3_CONTROL_H -#define SO3_CONTROL_H +#ifndef SO3_CONTROL_HPP +#define SO3_CONTROL_HPP #include @@ -27,7 +27,7 @@ class SO3Control const Eigen::Quaternionf &getComputedOrientation(); const Eigen::Vector3f &getComputedAngularVelocity(); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: // Inputs for the controller @@ -48,4 +48,4 @@ class SO3Control Eigen::Vector3f pos_int_b_; }; -#endif +#endif // SO3_CONTROL_HPP diff --git a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp index 55d57143..623d5071 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp @@ -1,16 +1,17 @@ -#include -#include -#include -#include -#include -#include -#include +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" -class SO3ControlTester +using namespace std::chrono_literals; + +class SO3ControlTester : public rclcpp::Node { public: SO3ControlTester(); - void so3CommandCallback(const kr_mav_msgs::SO3Command::ConstPtr &msg); + void so3CommandCallback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg); void publish_single_position_command(); void publish_enable_motors(bool flag); void populate_position_cmd_vector(int x_gain, int y_gain, int z_gain, uint8_t use_msg_gains_flags, float yaw_dot); @@ -20,27 +21,30 @@ class SO3ControlTester void reset_so3_cmd_pointer(); void publish_corrections(float kf_correction, float (&angle_corrections)[2]); bool is_so3_cmd_publisher_active(); - kr_mav_msgs::SO3Command::Ptr so3_cmd_; + kr_mav_msgs::msg::SO3Command::SharedPtr so3_cmd_; bool so3_command_received_ = false; std::mutex mutex; private: - ros::NodeHandle nh_; - ros::Publisher position_cmd_pub_, enable_motors_pub_, odom_pub_, corrections_pub_; - ros::Subscriber so3_cmd_sub_; - std::vector position_cmds; - std::vector odom_msgs; + rclcpp::Publisher::SharedPtr position_cmd_pub_; + rclcpp::Publisher::SharedPtr enable_motors_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr corrections_pub_; + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + std::vector position_cmds; + std::vector odom_msgs; std::array angles = {0.0, 0.314, 0.628, 0.942, 1.257, 1.571, 1.885, 2.199, 2.513, 2.827, 3.142}; }; -SO3ControlTester::SO3ControlTester() : nh_("") +SO3ControlTester::SO3ControlTester() : Node("so3_control_tester") { - position_cmd_pub_ = nh_.advertise("position_cmd", 5, true); - so3_cmd_sub_ = nh_.subscribe("so3_cmd", 5, &SO3ControlTester::so3CommandCallback, this); - enable_motors_pub_ = nh_.advertise("motors", 5, true); - odom_pub_ = nh_.advertise("odom", 5, true); - corrections_pub_ = nh_.advertise("corrections", 5, true); + position_cmd_pub_ = this->create_publisher("position_cmd", 5); + enable_motors_pub_ = this->create_publisher("motors", 5); + odom_pub_ = this->create_publisher("odom", 5); + corrections_pub_ = this->create_publisher("corrections", 5); + so3_cmd_sub_ = this->create_subscription( + "so3_cmd", 5, std::bind(&SO3ControlTester::so3CommandCallback, this, std::placeholders::_1)); } /* @@ -50,12 +54,12 @@ SO3ControlTester::SO3ControlTester() : nh_("") bool SO3ControlTester::is_so3_cmd_publisher_active() { bool flag = false; - if(so3_cmd_sub_.getNumPublishers() > 0) + if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else { - ros::Duration(1.0).sleep(); - if(so3_cmd_sub_.getNumPublishers() > 0) + rclcpp::sleep_for(1s); + if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else flag = false; @@ -66,20 +70,20 @@ bool SO3ControlTester::is_so3_cmd_publisher_active() /* * @brief Callback function to hand so3 command messages from nodelet. * - * @param msg Type: kr_mav_msgs::SO3Command::ConstPtr + * @param[in] msg Type: kr_mav_msgs::SO3Command::ConstPtr */ -void SO3ControlTester::so3CommandCallback(const kr_mav_msgs::SO3Command::ConstPtr &msg) +void SO3ControlTester::so3CommandCallback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) { std::lock_guard lock(mutex); so3_command_received_ = true; if(!so3_cmd_) { - so3_cmd_ = boost::make_shared(*msg); + so3_cmd_ = std::make_shared(*msg); } else { so3_cmd_.reset(); - so3_cmd_ = boost::make_shared(*msg); + so3_cmd_ = std::make_shared(*msg); } } @@ -99,7 +103,7 @@ void SO3ControlTester::reset_so3_cmd_pointer() */ void SO3ControlTester::publish_single_position_command() { - kr_mav_msgs::PositionCommand cmd; + auto cmd = kr_mav_msgs::msg::PositionCommand(); cmd.position.x = 1.0; cmd.position.y = 0.2; cmd.position.z = 5.0; @@ -121,30 +125,30 @@ void SO3ControlTester::publish_single_position_command() cmd.kv[1] = 0.1; cmd.kv[2] = 0.1; cmd.use_msg_gains_flags = 0; - position_cmd_pub_.publish(cmd); + position_cmd_pub_->publish(cmd); } /* * @brief Function to publish message over the topic `motors` * - * @param flag boolean flag message to enable/disable motors + * @param[in] flag boolean flag message to enable/disable motors */ void SO3ControlTester::publish_enable_motors(bool flag) { - std_msgs::Bool msg; + auto msg = std_msgs::msg::Bool(); msg.data = flag; - enable_motors_pub_.publish(msg); + enable_motors_pub_->publish(msg); } /* * @brief Function that populates the a vector of `kr_mav_messages::PositionCommand` * Generates position commands based on sine and cosine functions * - * @param x_gain int, max x-position - * @param y_gain int, max y-position - * @param z_gain int, max z_position - * @param use_msg_gains_flags uint8_t, indicates which gains to use from the message - * @param yaw_dot float, desired yaw_dot + * @param[in] x_gain int, max x-position + * @param[in] y_gain int, max y-position + * @param[in] z_gain int, max z_position + * @param[in] use_msg_gains_flags uint8_t, indicates which gains to use from the message + * @param[in] yaw_dot float, desired yaw_dot */ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int z_gain, uint8_t use_msg_gains_flags, float yaw_dot) @@ -160,7 +164,7 @@ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int float z_sin = z_gain * std::sin(angles[i]); float z_cos = z_gain * std::cos(angles[i]); - kr_mav_msgs::PositionCommand cmd; + auto cmd = kr_mav_msgs::msg::PositionCommand(); cmd.position.x = x_sin; cmd.position.y = y_sin; cmd.position.z = z_sin; @@ -189,14 +193,14 @@ void SO3ControlTester::populate_position_cmd_vector(int x_gain, int y_gain, int /* * @brief Function to publish position command indexed from the vector of `kr_mav_msgs::PositionCommand` msgs * - * @param index int + * @param[in] index int */ void SO3ControlTester::publish_position_command(int index) { int position_cmds_vector_size = position_cmds.size(); if(index < position_cmds_vector_size) { - position_cmd_pub_.publish(position_cmds[index]); + position_cmd_pub_->publish(position_cmds[index]); } } @@ -207,7 +211,7 @@ void SO3ControlTester::populate_odom_msgs() { odom_msgs.clear(); - nav_msgs::Odometry msg; + auto msg = nav_msgs::msg::Odometry(); msg.header.frame_id = "quadrotor"; msg.pose.pose.position.x = 0.0; msg.pose.pose.position.y = 0.0; @@ -260,30 +264,30 @@ void SO3ControlTester::populate_odom_msgs() /* * @brief Function to publish odom message index from a vector of `nav_msgs::Odometry` msgs * - * @param index int + * @param[in] index int */ void SO3ControlTester::publish_odom_msg(int index) { int odom_vector_size = odom_msgs.size(); if(index < odom_vector_size) { - odom_pub_.publish(odom_msgs[index]); + odom_pub_->publish(odom_msgs[index]); } } /* * @brief Function to publish corrections message of type `kr_mav_msgs::Corrections` * - * @param kf_correction float - * @param angle_corrections float[2] + * @param[in] kf_correction float + * @param[in] angle_corrections float[2] */ void SO3ControlTester::publish_corrections(float kf_correction, float (&angle_corrections)[2]) { - kr_mav_msgs::Corrections msg; + auto msg = kr_mav_msgs::msg::Corrections(); msg.kf_correction = kf_correction; msg.angle_corrections[0] = angle_corrections[0]; msg.angle_corrections[1] = angle_corrections[1]; - corrections_pub_.publish(msg); + corrections_pub_->publish(msg); } /* diff --git a/kr_mav_controllers/nodelet_plugin.xml b/kr_mav_controllers/nodelet_plugin.xml deleted file mode 100644 index f95a1e82..00000000 --- a/kr_mav_controllers/nodelet_plugin.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - kr_mav_so3_controllers - - - - - - kr_mav_so3_controllers with thrust, roll, pitch, yaw output - - - - - diff --git a/kr_mav_controllers/package.xml b/kr_mav_controllers/package.xml index 4008c415..69541ff2 100644 --- a/kr_mav_controllers/package.xml +++ b/kr_mav_controllers/package.xml @@ -1,30 +1,31 @@ - + + + kr_mav_controllers 1.0.0 kr_mav_controllers Kartik Mohta Michael Watterson - + Pranav Shah BSD Kartik Mohta - catkin + ament_cmake - dynamic_reconfigure - geometry_msgs + rclcpp + rclcpp_components kr_mav_msgs nav_msgs - nodelet - roscpp std_msgs - tf + geometry_msgs + tf2 + tf2_geometry_msgs - rostest - gtest + ament_cmake_gtest + launch_testing_ament_cmake - + ament_cmake - diff --git a/kr_mav_controllers/src/PIDControl.cpp b/kr_mav_controllers/src/PIDControl.cpp index 4bfb2a5f..526c1352 100644 --- a/kr_mav_controllers/src/PIDControl.cpp +++ b/kr_mav_controllers/src/PIDControl.cpp @@ -1,5 +1,6 @@ +#include "kr_mav_controllers/PIDControl.hpp" + #include -#include PIDControl::PIDControl() : mass_(0.5), g_(9.81), yaw_int_(0.0), max_pos_int_(0.5) {} diff --git a/kr_mav_controllers/src/SO3Control.cpp b/kr_mav_controllers/src/SO3Control.cpp index d9da233b..861b45e1 100644 --- a/kr_mav_controllers/src/SO3Control.cpp +++ b/kr_mav_controllers/src/SO3Control.cpp @@ -1,7 +1,4 @@ -#include "kr_mav_controllers/SO3Control.h" - -#include -#include +#include "kr_mav_controllers/SO3Control.hpp" SO3Control::SO3Control() : mass_(0.5), @@ -73,8 +70,6 @@ void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::V else if(pos_int_(i) < -max_pos_int_) pos_int_(i) = -max_pos_int_; } - // ROS_DEBUG_THROTTLE(2, "Integrated world disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", pos_int_(0), - // pos_int_(1), pos_int_(2)); Eigen::Quaternionf q(current_orientation_); const Eigen::Vector3f e_pos_b = q.inverse() * e_pos; @@ -89,8 +84,6 @@ void SO3Control::calculateControl(const Eigen::Vector3f &des_pos, const Eigen::V else if(pos_int_b_(i) < -max_pos_int_b_) pos_int_b_(i) = -max_pos_int_b_; } - // ROS_DEBUG_THROTTLE(2, "Integrated body disturbance compensation [N]: {x: %2.2f, y: %2.2f, z: %2.2f}", - // pos_int_b_(0), pos_int_b_(1), pos_int_b_(2)); const Eigen::Vector3f acc_grav = g_ * Eigen::Vector3f::UnitZ(); const Eigen::Vector3f acc_control = kx.asDiagonal() * e_pos + kv.asDiagonal() * e_vel + pos_int_ + des_acc; diff --git a/kr_mav_controllers/src/pid_control_component.cpp b/kr_mav_controllers/src/pid_control_component.cpp new file mode 100644 index 00000000..35381fa3 --- /dev/null +++ b/kr_mav_controllers/src/pid_control_component.cpp @@ -0,0 +1,184 @@ +#include + +#include "kr_mav_controllers/PIDControl.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/trpy_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) + +class PIDControlComponent : public rclcpp::Node +{ + public: + PIDControlComponent(const rclcpp::NodeOptions &options); + + private: + void publishTRPYCommand(void); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + + PIDControl controller_; + rclcpp::Publisher::SharedPtr trpy_command_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + + bool position_cmd_updated_, position_cmd_init_; + std::string frame_id_; + + Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_; + float des_yaw_; + float current_yaw_; + float ki_yaw_; + float max_roll_pitch_; + bool enable_motors_; + bool use_external_yaw_; +}; + +void PIDControlComponent::publishTRPYCommand(void) +{ + float ki_yaw = 0; + Eigen::Vector3f ki = Eigen::Vector3f::Zero(); + // Only enable integral terms when motors are on + if(enable_motors_) + { + ki_yaw = ki_yaw_; + ki = ki_; + } + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw); + + const Eigen::Vector4f &trpy = controller_.getControls(); + + auto trpy_command = std::make_unique(); + trpy_command->header.stamp = this->now(); + trpy_command->header.frame_id = frame_id_; + if(enable_motors_) + { + trpy_command->thrust = trpy(0); + trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_); + trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_); + trpy_command->yaw = trpy(3); + } + trpy_command->aux.current_yaw = current_yaw_; + trpy_command->aux.enable_motors = enable_motors_; + trpy_command->aux.use_external_yaw = use_external_yaw_; + trpy_command_pub_->publish(std::move(trpy_command)); +} + +void PIDControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) +{ + des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); + des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); + des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); + kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]); + kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]); + + des_yaw_ = cmd->yaw; + position_cmd_updated_ = true; + // position_cmd_init_ = true; + + publishTRPYCommand(); +} + +void PIDControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); + const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); + + current_yaw_ = tf2::getYaw(odom->pose.pose.orientation); + + controller_.setPosition(position); + controller_.setVelocity(velocity); + controller_.setYaw(current_yaw_); + + if(position_cmd_init_) + { + // We set position_cmd_updated_ = false and expect that the + // position_cmd_callback would set it to true since typically a position_cmd + // message would follow an odom message. If not, the position_cmd_callback + // hasn't been called and we publish the so3 command ourselves + // TODO: Fallback to hover if position_cmd hasn't been received for some time + if(!position_cmd_updated_) + publishTRPYCommand(); + position_cmd_updated_ = false; + } +} + +void PIDControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) +{ + if(msg->data) + RCLCPP_INFO(this->get_logger(), "Enabling Motors"); + else + RCLCPP_INFO(this->get_logger(), "Disabling Motors"); + + enable_motors_ = msg->data; + + // Reset integrals when toggling motor state + controller_.resetIntegrals(); +} + +PIDControlComponent::PIDControlComponent(const rclcpp::NodeOptions &options) + : Node("pid_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + current_yaw_(0), + enable_motors_(false), + use_external_yaw_(false) +{ + // RCLCPP_INFO_STREAM(this->get_logger(), "Intra-Process is "<< ( this->get_node_options().use_intra_process_comms() ? + // "ON" : "OFF" )); + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.235); + this->declare_parameter("use_external_yaw", true); + this->declare_parameter("max_roll_pitch", 30.0); + this->declare_parameter("gains/ki/x", 0.0); + this->declare_parameter("gains/ki/y", 0.0); + this->declare_parameter("gains/ki/z", 0.0); + this->declare_parameter("gains/ki/yaw", 0.01); + + std::string quadrotor_name; + this->get_parameter("quadrotor_name", quadrotor_name); + frame_id_ = "/" + quadrotor_name; + + double mass; + this->get_parameter("mass", mass); + controller_.setMass(mass); + controller_.setMaxIntegral(mass * 3); + + this->get_parameter("use_external_yaw", use_external_yaw_); + + double max_roll_pitch; + this->get_parameter("max_roll_pitch", max_roll_pitch); + max_roll_pitch_ = max_roll_pitch; + + double ki_x, ki_y, ki_z, ki_yaw; + this->get_parameter("gains/ki/x", ki_x); + this->get_parameter("gains/ki/y", ki_y); + this->get_parameter("gains/ki/z", ki_z); + this->get_parameter("gains/ki/yaw", ki_yaw); + ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z; + ki_yaw_ = ki_yaw; + + trpy_command_pub_ = this->create_publisher("~/trpy_cmd", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&PIDControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&PIDControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&PIDControlComponent::enable_motors_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(PIDControlComponent) diff --git a/kr_mav_controllers/src/pid_control_nodelet.cpp b/kr_mav_controllers/src/pid_control_nodelet.cpp deleted file mode 100644 index a6de961b..00000000 --- a/kr_mav_controllers/src/pid_control_nodelet.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) - -class PIDControlNodelet : public nodelet::Nodelet -{ - public: - PIDControlNodelet() - : position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - current_yaw_(0), - enable_motors_(false), - use_external_yaw_(false) - { - } - - void onInit(void); - - private: - void publishTRPYCommand(void); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - - PIDControl controller_; - ros::Publisher trpy_command_pub_; - ros::Subscriber odom_sub_; - ros::Subscriber position_cmd_sub_; - ros::Subscriber enable_motors_sub_; - - bool position_cmd_updated_, position_cmd_init_; - std::string frame_id_; - - Eigen::Vector3f des_pos_, des_vel_, des_acc_, kx_, kv_, ki_; - float des_yaw_; - float current_yaw_; - float ki_yaw_; - float max_roll_pitch_; - bool enable_motors_; - bool use_external_yaw_; -}; - -void PIDControlNodelet::publishTRPYCommand(void) -{ - float ki_yaw = 0; - Eigen::Vector3f ki = Eigen::Vector3f::Zero(); - // Only enable integral terms when motors are on - if(enable_motors_) - { - ki_yaw = ki_yaw_; - ki = ki_; - } - controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_, kx_, kv_, ki, ki_yaw); - - const Eigen::Vector4f &trpy = controller_.getControls(); - - kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand); - trpy_command->header.stamp = ros::Time::now(); - trpy_command->header.frame_id = frame_id_; - if(enable_motors_) - { - trpy_command->thrust = trpy(0); - trpy_command->roll = CLAMP(trpy(1), -max_roll_pitch_, max_roll_pitch_); - trpy_command->pitch = CLAMP(trpy(2), -max_roll_pitch_, max_roll_pitch_); - trpy_command->yaw = trpy(3); - } - trpy_command->aux.current_yaw = current_yaw_; - trpy_command->aux.enable_motors = enable_motors_; - trpy_command->aux.use_external_yaw = use_external_yaw_; - trpy_command_pub_.publish(trpy_command); -} - -void PIDControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ - des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); - des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); - des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); - kx_ = Eigen::Vector3f(cmd->kx[0], cmd->kx[1], cmd->kx[2]); - kv_ = Eigen::Vector3f(cmd->kv[0], cmd->kv[1], cmd->kv[2]); - - des_yaw_ = cmd->yaw; - position_cmd_updated_ = true; - // position_cmd_init_ = true; - - publishTRPYCommand(); -} - -void PIDControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); - const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); - - current_yaw_ = tf::getYaw(odom->pose.pose.orientation); - - controller_.setPosition(position); - controller_.setVelocity(velocity); - controller_.setYaw(current_yaw_); - - if(position_cmd_init_) - { - // We set position_cmd_updated_ = false and expect that the - // position_cmd_callback would set it to true since typically a position_cmd - // message would follow an odom message. If not, the position_cmd_callback - // hasn't been called and we publish the so3 command ourselves - // TODO: Fallback to hover if position_cmd hasn't been received for some time - if(!position_cmd_updated_) - publishTRPYCommand(); - position_cmd_updated_ = false; - } -} - -void PIDControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) -{ - if(msg->data) - ROS_INFO("Enabling motors"); - else - ROS_INFO("Disabling motors"); - - enable_motors_ = msg->data; - - // Reset integrals when toggling motor state - controller_.resetIntegrals(); -} - -void PIDControlNodelet::onInit(void) -{ - ros::NodeHandle n(getPrivateNodeHandle()); - - std::string quadrotor_name; - n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); - frame_id_ = "/" + quadrotor_name; - - double mass; - n.param("mass", mass, 0.235); - controller_.setMass(mass); - controller_.setMaxIntegral(mass * 3); - - n.param("use_external_yaw", use_external_yaw_, true); - - double max_roll_pitch; - n.param("max_roll_pitch", max_roll_pitch, 30.0); - max_roll_pitch_ = max_roll_pitch; - - double ki_x, ki_y, ki_z, ki_yaw; - n.param("gains/ki/x", ki_x, 0.0); - n.param("gains/ki/y", ki_y, 0.0); - n.param("gains/ki/z", ki_z, 0.0); - n.param("gains/ki/yaw", ki_yaw, 0.01); - ki_[0] = ki_x, ki_[1] = ki_y, ki_[2] = ki_z; - ki_yaw_ = ki_yaw; - - trpy_command_pub_ = n.advertise("trpy_cmd", 10); - - odom_sub_ = n.subscribe("odom", 10, &PIDControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = n.subscribe("position_cmd", 10, &PIDControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - - enable_motors_sub_ = - n.subscribe("motors", 2, &PIDControlNodelet::enable_motors_callback, this, ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(PIDControlNodelet, nodelet::Nodelet); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp new file mode 100644 index 00000000..823a8946 --- /dev/null +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -0,0 +1,649 @@ +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "kr_mav_controllers/SO3Control.hpp" +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/so3_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + +class SO3ControlComponent : public rclcpp::Node +{ + public: + SO3ControlComponent(const rclcpp::NodeOptions &options); + + private: + void publishSO3Command(); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + void corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg); + rcl_interfaces::msg::SetParametersResult cfg_callback(std::vector parameters); + + SO3Control controller_; + rclcpp::Publisher::SharedPtr so3_command_pub_; + rclcpp::Publisher::SharedPtr command_viz_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + rclcpp::Subscription::SharedPtr corrections_sub_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfigure_handle_; + + bool position_cmd_updated_, position_cmd_init_; + std::string frame_id_; + + Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, kx_, kv_, config_kx_, config_kv_, config_ki_, config_kib_; + float des_yaw_, des_yaw_dot_; + float current_yaw_; + bool enable_motors_, use_external_yaw_, have_odom_; + float kr_[3], kom_[3], corrections_[3]; + float mass_; + const float g_; + Eigen::Quaternionf current_orientation_; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Need this since we have SO3Control which needs aligned pointer +}; + +void SO3ControlComponent::publishSO3Command() +{ + if(!have_odom_) + { + RCLCPP_WARN(this->get_logger(), "No Odometry! Not publishing SO3Command."); + return; + } + + Eigen::Vector3f ki = Eigen::Vector3f::Zero(); + Eigen::Vector3f kib = Eigen::Vector3f::Zero(); + if(enable_motors_) + { + ki = config_ki_; + kib = config_kib_; + } + + controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); + + const Eigen::Vector3f &force = controller_.getComputedForce(); + const Eigen::Quaternionf &orientation = controller_.getComputedOrientation(); + const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity(); + + auto so3_command = std::make_unique(); + so3_command->header.stamp = this->now(); + so3_command->header.frame_id = frame_id_; + so3_command->force.x = force(0); + so3_command->force.y = force(1); + so3_command->force.z = force(2); + so3_command->orientation.x = orientation.x(); + so3_command->orientation.y = orientation.y(); + so3_command->orientation.z = orientation.z(); + so3_command->orientation.w = orientation.w(); + so3_command->angular_velocity.x = ang_vel(0); + so3_command->angular_velocity.y = ang_vel(1); + so3_command->angular_velocity.z = ang_vel(2); + for(int i = 0; i < 3; i++) + { + so3_command->kr[i] = kr_[i]; + so3_command->kom[i] = kom_[i]; + } + so3_command->aux.current_yaw = current_yaw_; + so3_command->aux.kf_correction = corrections_[0]; + so3_command->aux.angle_corrections[0] = corrections_[1]; + so3_command->aux.angle_corrections[1] = corrections_[2]; + so3_command->aux.enable_motors = enable_motors_; + so3_command->aux.use_external_yaw = use_external_yaw_; + + auto cmd_viz_msg = std::make_unique(); + cmd_viz_msg->header = so3_command->header; + cmd_viz_msg->pose.position.x = des_pos_(0); + cmd_viz_msg->pose.position.y = des_pos_(1); + cmd_viz_msg->pose.position.z = des_pos_(2); + cmd_viz_msg->pose.orientation.x = orientation.x(); + cmd_viz_msg->pose.orientation.y = orientation.y(); + cmd_viz_msg->pose.orientation.z = orientation.z(); + cmd_viz_msg->pose.orientation.w = orientation.w(); + command_viz_pub_->publish(std::move(cmd_viz_msg)); + + so3_command_pub_->publish(std::move(so3_command)); +} + +void SO3ControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) +{ + des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); + des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); + des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); + des_jrk_ = Eigen::Vector3f(cmd->jerk.x, cmd->jerk.y, cmd->jerk.z); + + // Check use_msg_gains_flag to decide whether to use gains from the msg or config + kx_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_X) ? cmd->kx[0] : config_kx_[0]; + kx_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Y) ? cmd->kx[1] : config_kx_[1]; + kx_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Z) ? cmd->kx[2] : config_kx_[2]; + kv_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_X) ? cmd->kv[0] : config_kv_[0]; + kv_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Y) ? cmd->kv[1] : config_kv_[1]; + kv_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Z) ? cmd->kv[2] : config_kv_[2]; + + des_yaw_ = cmd->yaw; + des_yaw_dot_ = cmd->yaw_dot; + position_cmd_updated_ = true; + // position_cmd_init_ = true; + + publishSO3Command(); +} + +void SO3ControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) +{ + have_odom_ = true; + + frame_id_ = odom->header.frame_id; + + const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); + const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); + + current_yaw_ = tf2::getYaw(odom->pose.pose.orientation); + + current_orientation_ = Eigen::Quaternionf(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); + + controller_.setPosition(position); + controller_.setVelocity(velocity); + controller_.setCurrentOrientation(current_orientation_); + + if(position_cmd_init_) + { + // We set position_cmd_updated_ = false and expect that the + // position_cmd_callback would set it to true since typically a position_cmd + // message would follow an odom message. If not, the position_cmd_callback + // hasn't been called and we publish the so3 command ourselves + // TODO: Fallback to hover if position_cmd hasn't been received for some time + if(!position_cmd_updated_) + publishSO3Command(); + position_cmd_updated_ = false; + } +} + +void SO3ControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) +{ + if(msg->data) + RCLCPP_INFO(this->get_logger(), "Enabling motors"); + else + RCLCPP_INFO(this->get_logger(), "Disabling motors"); + + enable_motors_ = msg->data; + // Reset integral when toggling motor state + controller_.resetIntegrals(); +} + +void SO3ControlComponent::corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg) +{ + corrections_[0] = msg->kf_correction; + corrections_[1] = msg->angle_corrections[0]; + corrections_[2] = msg->angle_corrections[1]; +} + +rcl_interfaces::msg::SetParametersResult SO3ControlComponent::cfg_callback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for(auto ¶meter : parameters) + { + const auto &name = parameter.get_name(); + + if(name == "kp_x") + { + config_kx_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_y") + { + config_kx_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_z") + { + config_kx_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_x") + { + config_kv_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_y") + { + config_kv_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_z") + { + config_kv_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "ki_x") + { + config_ki_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_ki_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_ki_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "kib_x") + { + config_kib_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_kib_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_kib_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "rot_x") + { + kr_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_y") + { + kr_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_z") + { + kr_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_x") + { + kom_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_y") + { + kom_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_z") + { + kom_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "kf_correction") + { + corrections_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "roll_correction") + { + corrections_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "pitch_correction") + { + corrections_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "max_pos_int") + { + controller_.setMaxIntegral(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "World max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_pos_int_b") + { + controller_.setMaxIntegralBody(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Body max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_tilt_angle") + { + controller_.setMaxTiltAngle(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Max tilt angle set to: %2.2g", parameter.as_double()); + } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "kr_mav_controllers dynamic reconfigure called with invalid parameter"); + result.successful = false; + return result; + } + } + result.successful = true; + return result; +} + +SO3ControlComponent::SO3ControlComponent(const rclcpp::NodeOptions &options) + : Node("so3_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + des_yaw_dot_(0), + current_yaw_(0), + enable_motors_(false), + use_external_yaw_(false), + have_odom_(false), + g_(9.81), + current_orientation_(Eigen::Quaternionf::Identity()) +{ + controller_.resetIntegrals(); + + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.5f); + + std::string quadrotor_name; + this->get_parameter("quadrotor_name", quadrotor_name); + frame_id_ = "/" + quadrotor_name; + this->get_parameter("mass", mass_); + + controller_.setMass(mass_); + controller_.setGravity(g_); + + this->declare_parameter("use_external_yaw", true); + this->get_parameter("use_external_yaw", use_external_yaw_); + + // Dynamic parameter reconfiguration definitions + + // Translation Gains: World Position Gains + rcl_interfaces::msg::ParameterDescriptor desc_kp_x; + rcl_interfaces::msg::FloatingPointRange r_kp_x; + r_kp_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_x.type = 3; // PARAMETER_DOUBLE + desc_kp_x.description = std::string("World x position gain"); + desc_kp_x.floating_point_range = {r_kp_x}; + this->declare_parameter("kp_x", 7.4f, desc_kp_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_y; + rcl_interfaces::msg::FloatingPointRange r_kp_y; + r_kp_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_y.type = 3; // PARAMETER_DOUBLE + desc_kp_y.description = std::string("World y position gain"); + desc_kp_y.floating_point_range = {r_kp_y}; + this->declare_parameter("kp_y", 7.4f, desc_kp_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_z; + rcl_interfaces::msg::FloatingPointRange r_kp_z; + r_kp_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_z.type = 3; // PARAMETER_DOUBLE + desc_kp_z.description = std::string("World z position gain"); + desc_kp_z.floating_point_range = {r_kp_z}; + this->declare_parameter("kp_z", 10.4f, desc_kp_z); + + // Translation Gains: World Derivative Gains + rcl_interfaces::msg::ParameterDescriptor desc_kd_x; + rcl_interfaces::msg::FloatingPointRange r_kd_x; + r_kd_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_x.type = 3; // PARAMETER_DOUBLE + desc_kd_x.description = std::string("World x derivative gain"); + desc_kd_x.floating_point_range = {r_kd_x}; + this->declare_parameter("kd_x", 4.8f, desc_kd_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_y; + rcl_interfaces::msg::FloatingPointRange r_kd_y; + r_kd_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_y.type = 3; // PARAMETER_DOUBLE + desc_kd_y.description = std::string("World y derivative gain"); + desc_kd_y.floating_point_range = {r_kd_y}; + this->declare_parameter("kd_y", 4.8f, desc_kd_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_z; + rcl_interfaces::msg::FloatingPointRange r_kd_z; + r_kd_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_z.type = 3; // PARAMETER_DOUBLE + desc_kd_z.description = std::string("World z derivative gain"); + desc_kd_z.floating_point_range = {r_kd_z}; + this->declare_parameter("kd_z", 6.0f, desc_kd_z); + + // Integral Gains: World Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_ki_x; + rcl_interfaces::msg::FloatingPointRange r_ki_x; + r_ki_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_x.type = 3; // PARAMETER_DOUBLE + desc_ki_x.description = std::string("World x integral gain"); + desc_ki_x.floating_point_range = {r_ki_x}; + this->declare_parameter("ki_x", 0.0f, desc_ki_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_y; + rcl_interfaces::msg::FloatingPointRange r_ki_y; + r_ki_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_y.type = 3; // PARAMETER_DOUBLE + desc_ki_y.description = std::string("World y integral gain"); + desc_ki_y.floating_point_range = {r_ki_y}; + this->declare_parameter("ki_y", 0.0f, desc_ki_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_z; + rcl_interfaces::msg::FloatingPointRange r_ki_z; + r_ki_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_z.type = 3; // PARAMETER_DOUBLE + desc_ki_z.description = std::string("World z integral gain"); + desc_ki_z.floating_point_range = {r_ki_z}; + this->declare_parameter("ki_z", 0.0f, desc_ki_z); + + // Integral Gains: Body Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_kib_x; + rcl_interfaces::msg::FloatingPointRange r_kib_x; + r_kib_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_x.type = 3; // PARAMETER_DOUBLE + desc_kib_x.description = std::string("Body x integral gain"); + desc_kib_x.floating_point_range = {r_kib_x}; + this->declare_parameter("kib_x", 0.0f, desc_kib_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_y; + rcl_interfaces::msg::FloatingPointRange r_kib_y; + r_kib_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_y.type = 3; // PARAMETER_DOUBLE + desc_kib_y.description = std::string("Body y integral gain"); + desc_kib_y.floating_point_range = {r_kib_y}; + this->declare_parameter("kib_y", 0.0f, desc_kib_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_z; + rcl_interfaces::msg::FloatingPointRange r_kib_z; + r_kib_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_z.type = 3; // PARAMETER_DOUBLE + desc_kib_z.description = std::string("Body z integral gain"); + desc_kib_z.floating_point_range = {r_kib_z}; + this->declare_parameter("kib_z", 0.0f, desc_kib_z); + + // Attitude Gains: Rotation Gains + rcl_interfaces::msg::ParameterDescriptor desc_rot_x; + rcl_interfaces::msg::FloatingPointRange r_rot_x; + r_rot_x.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_x.type = 3; // PARAMETER_DOUBLE + desc_rot_x.description = std::string("Rotation x gain"); + desc_rot_x.floating_point_range = {r_rot_x}; + this->declare_parameter("rot_x", 1.5f, desc_rot_x); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_y; + rcl_interfaces::msg::FloatingPointRange r_rot_y; + r_rot_y.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_y.type = 3; // PARAMETER_DOUBLE + desc_rot_y.description = std::string("Rotation y gain"); + desc_rot_y.floating_point_range = {r_rot_y}; + this->declare_parameter("rot_y", 1.5f, desc_rot_y); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_z; + rcl_interfaces::msg::FloatingPointRange r_rot_z; + r_rot_z.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_z.type = 3; // PARAMETER_DOUBLE + desc_rot_z.description = std::string("Rotation z gain"); + desc_rot_z.floating_point_range = {r_rot_z}; + this->declare_parameter("rot_z", 1.0f, desc_rot_z); + + // Attitude Gains: Angular Gains + rcl_interfaces::msg::ParameterDescriptor desc_ang_x; + rcl_interfaces::msg::FloatingPointRange r_ang_x; + r_ang_x.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_x.type = 3; // PARAMETER_DOUBLE + desc_ang_x.description = std::string("Angular x gain"); + desc_ang_x.floating_point_range = {r_ang_x}; + this->declare_parameter("ang_x", 0.13f, desc_ang_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_y; + rcl_interfaces::msg::FloatingPointRange r_ang_y; + r_ang_y.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_y.type = 3; // PARAMETER_DOUBLE + desc_ang_y.description = std::string("Angular y gain"); + desc_ang_y.floating_point_range = {r_ang_y}; + this->declare_parameter("ang_y", 0.13f, desc_ang_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_z; + rcl_interfaces::msg::FloatingPointRange r_ang_z; + r_ang_z.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_z.type = 3; // PARAMETER_DOUBLE + desc_ang_z.description = std::string("Angular z gain"); + desc_ang_z.floating_point_range = {r_ang_z}; + this->declare_parameter("ang_z", 0.10f, desc_ang_z); + + // Low level Corrections + rcl_interfaces::msg::ParameterDescriptor desc_kf_correction; + desc_kf_correction.type = 3; // PARAMETER_DOUBLE + desc_kf_correction.description = std::string("kf"); + this->declare_parameter("kf_correction", 0.0f, desc_kf_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_roll_correction; + desc_roll_correction.type = 3; // PARAMETER_DOUBLE + desc_roll_correction.description = std::string("roll"); + this->declare_parameter("roll_correction", 0.0f, desc_roll_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_pitch_correction; + desc_pitch_correction.type = 3; // PARAMETER_DOUBLE + desc_pitch_correction.description = std::string("pitch"); + this->declare_parameter("pitch_correction", 0.0f, desc_pitch_correction); + + // Misc Parameters + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int; + r_max_pos_int.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int.description = std::string("World max integral"); + desc_max_pos_int.floating_point_range = {r_max_pos_int}; + this->declare_parameter("max_pos_int", 0.5f, desc_max_pos_int); + + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int_b; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int_b; + r_max_pos_int_b.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int_b.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int_b.description = std::string("Body max integral"); + desc_max_pos_int_b.floating_point_range = {r_max_pos_int_b}; + this->declare_parameter("max_pos_int_b", 0.5f, desc_max_pos_int_b); + + rcl_interfaces::msg::ParameterDescriptor desc_max_tilt_angle; + rcl_interfaces::msg::FloatingPointRange r_max_tilt_angle; + r_max_tilt_angle.set__from_value(0.0).set__to_value(static_cast(M_PI)).set__step(0); + desc_max_tilt_angle.type = 3; // PARAMETER_DOUBLE + desc_max_tilt_angle.description = std::string("Max tilt angle"); + desc_max_tilt_angle.floating_point_range = {r_max_tilt_angle}; + this->declare_parameter("max_tilt_angle", 3.14f, desc_max_tilt_angle); + + // Getting dynamic parameters + this->get_parameter("kp_x", config_kx_[0]); + this->get_parameter("kp_y", config_kx_[1]); + this->get_parameter("kp_z", config_kx_[2]); + kx_[0] = config_kx_[0]; + kx_[1] = config_kx_[1]; + kx_[2] = config_kx_[2]; + + this->get_parameter("kd_x", config_kv_[0]); + this->get_parameter("kd_y", config_kv_[1]); + this->get_parameter("kd_z", config_kv_[2]); + kv_[0] = config_kv_[0]; + kv_[1] = config_kv_[1]; + kv_[2] = config_kv_[2]; + + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + + this->get_parameter("ki_x", config_ki_[0]); + this->get_parameter("ki_y", config_ki_[1]); + this->get_parameter("ki_z", config_ki_[2]); + + this->get_parameter("kib_x", config_kib_[0]); + this->get_parameter("kib_y", config_kib_[1]); + this->get_parameter("kib_z", config_kib_[2]); + + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + + this->get_parameter("rot_x", kr_[0]); + this->get_parameter("rot_y", kr_[1]); + this->get_parameter("rot_z", kr_[2]); + + this->get_parameter("ang_x", kom_[0]); + this->get_parameter("ang_y", kom_[1]); + this->get_parameter("ang_z", kom_[2]); + + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kr_[0], + kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + + this->get_parameter("kf_correction", corrections_[0]); + this->get_parameter("roll_correction", corrections_[1]); + this->get_parameter("pitch_correction", corrections_[2]); + + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + + float max_pos_int, max_pos_int_b; + this->get_parameter("max_pos_int", max_pos_int); + this->get_parameter("max_pos_int_b", max_pos_int_b); + controller_.setMaxIntegral(max_pos_int); + controller_.setMaxIntegralBody(max_pos_int_b); + + float max_tilt_angle; + this->get_parameter("max_tilt_angle", max_tilt_angle); + controller_.setMaxTiltAngle(max_tilt_angle); + + RCLCPP_INFO(this->get_logger(), "Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (Rad): %2.2g", + max_pos_int, max_pos_int_b, max_tilt_angle); + + // Initialize dynamic reconfigure callback + reconfigure_handle_ = + this->add_on_set_parameters_callback(std::bind(&SO3ControlComponent::cfg_callback, this, std::placeholders::_1)); + + so3_command_pub_ = this->create_publisher("~/so3_cmd", 10); + command_viz_pub_ = this->create_publisher("~/cmd_viz", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3ControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&SO3ControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&SO3ControlComponent::enable_motors_callback, this, std::placeholders::_1)); + corrections_sub_ = this->create_subscription( + "~/corrections", qos1, std::bind(&SO3ControlComponent::corrections_callback, this, std::placeholders::_1)); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3ControlComponent) diff --git a/kr_mav_controllers/src/so3_control_nodelet.cpp b/kr_mav_controllers/src/so3_control_nodelet.cpp deleted file mode 100644 index 38ef3e56..00000000 --- a/kr_mav_controllers/src/so3_control_nodelet.cpp +++ /dev/null @@ -1,376 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -class SO3ControlNodelet : public nodelet::Nodelet -{ - public: - SO3ControlNodelet() - : position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - des_yaw_dot_(0), - current_yaw_(0), - enable_motors_(false), - use_external_yaw_(false), - have_odom_(false), - g_(9.81), - current_orientation_(Eigen::Quaternionf::Identity()) - { - controller_.resetIntegrals(); - } - - void onInit(); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Need this since we have SO3Control which needs aligned pointer - - private: - void publishSO3Command(); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - void corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg); - void cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level); - - SO3Control controller_; - ros::Publisher so3_command_pub_, command_viz_pub_; - ros::Subscriber odom_sub_, position_cmd_sub_, enable_motors_sub_, corrections_sub_; - - bool position_cmd_updated_, position_cmd_init_; - std::string frame_id_; - - Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, config_kx_, config_kv_, config_ki_, config_kib_, kx_, kv_; - float des_yaw_, des_yaw_dot_; - float current_yaw_; - bool enable_motors_, use_external_yaw_, have_odom_; - float kR_[3], kOm_[3], corrections_[3]; - float mass_; - const float g_; - Eigen::Quaternionf current_orientation_; - - boost::recursive_mutex config_mutex_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; -}; - -void SO3ControlNodelet::publishSO3Command() -{ - if(!have_odom_) - { - ROS_WARN("No odometry! Not publishing SO3Command."); - return; - } - - Eigen::Vector3f ki = Eigen::Vector3f::Zero(); - Eigen::Vector3f kib = Eigen::Vector3f::Zero(); - if(enable_motors_) - { - ki = config_ki_; - kib = config_kib_; - } - - controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); - - const Eigen::Vector3f &force = controller_.getComputedForce(); - const Eigen::Quaternionf &orientation = controller_.getComputedOrientation(); - const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity(); - - kr_mav_msgs::SO3Command::Ptr so3_command = boost::make_shared(); - so3_command->header.stamp = ros::Time::now(); - so3_command->header.frame_id = frame_id_; - so3_command->force.x = force(0); - so3_command->force.y = force(1); - so3_command->force.z = force(2); - so3_command->orientation.x = orientation.x(); - so3_command->orientation.y = orientation.y(); - so3_command->orientation.z = orientation.z(); - so3_command->orientation.w = orientation.w(); - so3_command->angular_velocity.x = ang_vel(0); - so3_command->angular_velocity.y = ang_vel(1); - so3_command->angular_velocity.z = ang_vel(2); - for(int i = 0; i < 3; i++) - { - so3_command->kR[i] = kR_[i]; - so3_command->kOm[i] = kOm_[i]; - } - so3_command->aux.current_yaw = current_yaw_; - so3_command->aux.kf_correction = corrections_[0]; - so3_command->aux.angle_corrections[0] = corrections_[1]; - so3_command->aux.angle_corrections[1] = corrections_[2]; - so3_command->aux.enable_motors = enable_motors_; - so3_command->aux.use_external_yaw = use_external_yaw_; - so3_command_pub_.publish(so3_command); - - geometry_msgs::PoseStamped::Ptr cmd_viz_msg = boost::make_shared(); - cmd_viz_msg->header = so3_command->header; - cmd_viz_msg->pose.position.x = des_pos_(0); - cmd_viz_msg->pose.position.y = des_pos_(1); - cmd_viz_msg->pose.position.z = des_pos_(2); - cmd_viz_msg->pose.orientation.x = orientation.x(); - cmd_viz_msg->pose.orientation.y = orientation.y(); - cmd_viz_msg->pose.orientation.z = orientation.z(); - cmd_viz_msg->pose.orientation.w = orientation.w(); - command_viz_pub_.publish(cmd_viz_msg); -} - -void SO3ControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ - des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); - des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); - des_acc_ = Eigen::Vector3f(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z); - des_jrk_ = Eigen::Vector3f(cmd->jerk.x, cmd->jerk.y, cmd->jerk.z); - - // Check use_msg_gains_flag to decide whether to use gains from the msg or config - kx_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_X) ? cmd->kx[0] : config_kx_[0]; - kx_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Y) ? cmd->kx[1] : config_kx_[1]; - kx_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_POSITION_Z) ? cmd->kx[2] : config_kx_[2]; - kv_[0] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_X) ? cmd->kv[0] : config_kv_[0]; - kv_[1] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Y) ? cmd->kv[1] : config_kv_[1]; - kv_[2] = (cmd->use_msg_gains_flags & cmd->USE_MSG_GAINS_VELOCITY_Z) ? cmd->kv[2] : config_kv_[2]; - - des_yaw_ = cmd->yaw; - des_yaw_dot_ = cmd->yaw_dot; - position_cmd_updated_ = true; - // position_cmd_init_ = true; - - publishSO3Command(); -} - -void SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) -{ - have_odom_ = true; - - frame_id_ = odom->header.frame_id; - - const Eigen::Vector3f position(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z); - const Eigen::Vector3f velocity(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z); - - current_yaw_ = tf::getYaw(odom->pose.pose.orientation); - - current_orientation_ = Eigen::Quaternionf(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - - controller_.setPosition(position); - controller_.setVelocity(velocity); - controller_.setCurrentOrientation(current_orientation_); - - if(position_cmd_init_) - { - // We set position_cmd_updated_ = false and expect that the - // position_cmd_callback would set it to true since typically a position_cmd - // message would follow an odom message. If not, the position_cmd_callback - // hasn't been called and we publish the so3 command ourselves - // TODO: Fallback to hover if position_cmd hasn't been received for some time - if(!position_cmd_updated_) - publishSO3Command(); - position_cmd_updated_ = false; - } -} - -void SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) -{ - if(msg->data) - ROS_INFO("Enabling motors"); - else - ROS_INFO("Disabling motors"); - - enable_motors_ = msg->data; - // Reset integral when toggling motor state - controller_.resetIntegrals(); -} - -void SO3ControlNodelet::corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg) -{ - corrections_[0] = msg->kf_correction; - corrections_[1] = msg->angle_corrections[0]; - corrections_[2] = msg->angle_corrections[1]; -} - -void SO3ControlNodelet::cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level) -{ - if(level == 0) - { - NODELET_DEBUG_STREAM("Nothing changed. level: " << level); - return; - } - - if(level & (1 << 0)) - { - config_kx_[0] = config.kp_x; - config_kx_[1] = config.kp_y; - config_kx_[2] = config.kp_z; - - config_kv_[0] = config.kd_x; - config_kv_[1] = config.kd_y; - config_kv_[2] = config.kd_z; - - NODELET_INFO("Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", config_kx_[0], - config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); - } - - if(level & (1 << 1)) - { - config_ki_[0] = config.ki_x; - config_ki_[1] = config.ki_y; - config_ki_[2] = config.ki_z; - - config_kib_[0] = config.kib_x; - config_kib_[1] = config.kib_y; - config_kib_[2] = config.kib_z; - - NODELET_INFO("Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", config_ki_[0], - config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); - } - - if(level & (1 << 2)) - { - kR_[0] = config.rot_x; - kR_[1] = config.rot_y; - kR_[2] = config.rot_z; - - kOm_[0] = config.ang_x; - kOm_[1] = config.ang_y; - kOm_[2] = config.ang_z; - - NODELET_INFO("Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kR_[0], kR_[1], kR_[2], - kOm_[0], kOm_[1], kOm_[2]); - } - - if(level & (1 << 3)) - { - corrections_[0] = config.kf_correction; - corrections_[1] = config.roll_correction; - corrections_[2] = config.pitch_correction; - NODELET_INFO("Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], corrections_[1], - corrections_[2]); - } - - if(level & (1 << 4)) - { - controller_.setMaxIntegral(config.max_pos_int); - controller_.setMaxIntegralBody(config.max_pos_int_b); - controller_.setMaxTiltAngle(config.max_tilt_angle); - - NODELET_INFO("Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (rad): %2.2g", config.max_pos_int, - config.max_pos_int_b, config.max_tilt_angle); - } - - NODELET_WARN_STREAM_COND(level != std::numeric_limits::max() && (level >= (1 << 5)), - "kr_mav_controllers dynamic reconfigure called, but with unknown level: " << level); -} - -void SO3ControlNodelet::onInit(void) -{ - ros::NodeHandle priv_nh(getPrivateNodeHandle()); - - std::string quadrotor_name; - priv_nh.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); - frame_id_ = "/" + quadrotor_name; - - priv_nh.param("mass", mass_, 0.5f); - controller_.setMass(mass_); - controller_.setGravity(g_); - - // Dynamic reconfigure struct - kr_mav_controllers::SO3Config config; - - priv_nh.param("use_external_yaw", use_external_yaw_, true); - - priv_nh.param("gains/pos/x", config_kx_[0], 7.4f); - priv_nh.param("gains/pos/y", config_kx_[1], 7.4f); - priv_nh.param("gains/pos/z", config_kx_[2], 10.4f); - kx_[0] = config_kx_[0]; - kx_[1] = config_kx_[1]; - kx_[2] = config_kx_[2]; - config.kp_x = config_kx_[0]; - config.kp_y = config_kx_[1]; - config.kp_z = config_kx_[2]; - - priv_nh.param("gains/vel/x", config_kv_[0], 4.8f); - priv_nh.param("gains/vel/y", config_kv_[1], 4.8f); - priv_nh.param("gains/vel/z", config_kv_[2], 6.0f); - kv_[0] = config_kv_[0]; - kv_[1] = config_kv_[1]; - kv_[2] = config_kv_[2]; - config.kd_x = config_kv_[0]; - config.kd_y = config_kv_[1]; - config.kd_z = config_kv_[2]; - - priv_nh.param("gains/ki/x", config_ki_[0], 0.0f); - priv_nh.param("gains/ki/y", config_ki_[1], 0.0f); - priv_nh.param("gains/ki/z", config_ki_[2], 0.0f); - config.ki_x = config_ki_[0]; - config.ki_y = config_ki_[1]; - config.ki_z = config_ki_[2]; - - priv_nh.param("gains/kib/x", config_kib_[0], 0.0f); - priv_nh.param("gains/kib/y", config_kib_[1], 0.0f); - priv_nh.param("gains/kib/z", config_kib_[2], 0.0f); - config.kib_x = config_kib_[0]; - config.kib_y = config_kib_[1]; - config.kib_z = config_kib_[2]; - - priv_nh.param("gains/rot/x", kR_[0], 1.5f); - priv_nh.param("gains/rot/y", kR_[1], 1.5f); - priv_nh.param("gains/rot/z", kR_[2], 1.0f); - priv_nh.param("gains/ang/x", kOm_[0], 0.13f); - priv_nh.param("gains/ang/y", kOm_[1], 0.13f); - priv_nh.param("gains/ang/z", kOm_[2], 0.1f); - config.rot_x = kR_[0]; - config.rot_y = kR_[1]; - config.rot_z = kR_[2]; - config.ang_x = kOm_[0]; - config.ang_y = kOm_[1]; - config.ang_z = kOm_[2]; - - priv_nh.param("corrections/kf", corrections_[0], 0.0f); - priv_nh.param("corrections/r", corrections_[1], 0.0f); - priv_nh.param("corrections/p", corrections_[2], 0.0f); - config.kf_correction = corrections_[0]; - config.roll_correction = corrections_[1]; - config.pitch_correction = corrections_[2]; - - float max_pos_int, max_pos_int_b; - priv_nh.param("max_pos_int", max_pos_int, 0.5f); - priv_nh.param("mas_pos_int_b", max_pos_int_b, 0.5f); - controller_.setMaxIntegral(max_pos_int); - controller_.setMaxIntegralBody(max_pos_int_b); - config.max_pos_int = max_pos_int; - config.max_pos_int_b = max_pos_int_b; - - float max_tilt_angle; - priv_nh.param("max_tilt_angle", max_tilt_angle, static_cast(M_PI)); - controller_.setMaxTiltAngle(max_tilt_angle); - config.max_tilt_angle = max_tilt_angle; - - // Initialize dynamic reconfigure - reconfigure_server_ = boost::make_shared(config_mutex_, priv_nh); - reconfigure_server_->updateConfig(config); - reconfigure_server_->setCallback(boost::bind(&SO3ControlNodelet::cfg_callback, this, _1, _2)); - - so3_command_pub_ = priv_nh.advertise("so3_cmd", 10); - command_viz_pub_ = priv_nh.advertise("cmd_viz", 10); - - odom_sub_ = - priv_nh.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = priv_nh.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - enable_motors_sub_ = priv_nh.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this, - ros::TransportHints().tcpNoDelay()); - corrections_sub_ = priv_nh.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback, this, - ros::TransportHints().tcpNoDelay()); -} - -#include -PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet); diff --git a/kr_mav_controllers/src/so3_trpy_control.cpp b/kr_mav_controllers/src/so3_trpy_control.cpp index 3ca01d50..3c4b750f 100644 --- a/kr_mav_controllers/src/so3_trpy_control.cpp +++ b/kr_mav_controllers/src/so3_trpy_control.cpp @@ -1,73 +1,56 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include +#include "kr_mav_controllers/SO3Control.hpp" +#include "kr_mav_msgs/msg/corrections.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_mav_msgs/msg/trpy_command.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + #define CLAMP(x, min, max) ((x) < (min)) ? (min) : ((x) > (max)) ? (max) : (x) -class SO3TRPYControlNodelet : public nodelet::Nodelet +class SO3TRPYControlComponent : public rclcpp::Node { public: - SO3TRPYControlNodelet() - : odom_set_(false), - position_cmd_updated_(false), - position_cmd_init_(false), - des_yaw_(0), - des_yaw_dot_(0), - yaw_int_(0), - enable_motors_(false), - use_external_yaw_(false), - g_(9.81) - { - controller_.resetIntegrals(); - } - - void onInit(); - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Need this since we have SO3Control which needs aligned pointer + SO3TRPYControlComponent(const rclcpp::NodeOptions &options); private: void publishCommand(); - void position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); - void enable_motors_callback(const std_msgs::Bool::ConstPtr &msg); - void corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg); - void cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level); + void position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd); + void odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom); + void enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg); + void corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg); + rcl_interfaces::msg::SetParametersResult cfg_callback(std::vector parameters); SO3Control controller_; - ros::Publisher trpy_command_pub_; - ros::Subscriber odom_sub_, position_cmd_sub_, enable_motors_sub_, corrections_sub_; + rclcpp::Publisher::SharedPtr trpy_command_pub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr position_cmd_sub_; + rclcpp::Subscription::SharedPtr enable_motors_sub_; + rclcpp::Subscription::SharedPtr corrections_sub_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr reconfigure_handle_; bool odom_set_, position_cmd_updated_, position_cmd_init_; std::string frame_id_; - Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, config_kx_, config_kv_, config_ki_, config_kib_, kx_, kv_; + Eigen::Vector3f des_pos_, des_vel_, des_acc_, des_jrk_, kx_, kv_, config_kx_, config_kv_, config_ki_, config_kib_; float des_yaw_, des_yaw_dot_, yaw_int_; bool enable_motors_, use_external_yaw_; - float kR_[3], kOm_[3], corrections_[3]; + float kr_[3], kom_[3], corrections_[3]; float ki_yaw_; float mass_; const float g_; Eigen::Quaternionf current_orientation_; - boost::recursive_mutex config_mutex_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Need this since we have SO3Control which needs aligned pointer }; -void SO3TRPYControlNodelet::publishCommand() +void SO3TRPYControlComponent::publishCommand() { if(!odom_set_) { - NODELET_INFO_THROTTLE(1, "Odom not set, not publishing command!"); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Odom no set, not publishing command!"); return; } @@ -123,8 +106,8 @@ void SO3TRPYControlNodelet::publishCommand() else if(yaw_cmd < -PI) yaw_cmd += 2 * PI; - kr_mav_msgs::TRPYCommand::Ptr trpy_command(new kr_mav_msgs::TRPYCommand); - trpy_command->header.stamp = ros::Time::now(); + auto trpy_command = std::make_unique(); + trpy_command->header.stamp = this->now(); trpy_command->header.frame_id = frame_id_; if(enable_motors_) { @@ -137,8 +120,8 @@ void SO3TRPYControlNodelet::publishCommand() trpy_command->angular_velocity.z = ang_vel(2); for(int i = 0; i < 3; i++) { - trpy_command->kR[i] = kR_[i]; - trpy_command->kOm[i] = kOm_[i]; + trpy_command->kr[i] = kr_[i]; + trpy_command->kom[i] = kom_[i]; } } trpy_command->aux.current_yaw = yaw_cur; @@ -147,10 +130,10 @@ void SO3TRPYControlNodelet::publishCommand() trpy_command->aux.angle_corrections[1] = corrections_[2]; trpy_command->aux.enable_motors = enable_motors_; trpy_command->aux.use_external_yaw = use_external_yaw_; - trpy_command_pub_.publish(trpy_command); + trpy_command_pub_->publish(std::move(trpy_command)); } -void SO3TRPYControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +void SO3TRPYControlComponent::position_cmd_callback(const kr_mav_msgs::msg::PositionCommand::UniquePtr cmd) { des_pos_ = Eigen::Vector3f(cmd->position.x, cmd->position.y, cmd->position.z); des_vel_ = Eigen::Vector3f(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z); @@ -173,7 +156,7 @@ void SO3TRPYControlNodelet::position_cmd_callback(const kr_mav_msgs::PositionCom publishCommand(); } -void SO3TRPYControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) +void SO3TRPYControlComponent::odom_callback(const nav_msgs::msg::Odometry::UniquePtr odom) { if(!odom_set_) odom_set_ = true; @@ -201,12 +184,12 @@ void SO3TRPYControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr &od } } -void SO3TRPYControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr &msg) +void SO3TRPYControlComponent::enable_motors_callback(const std_msgs::msg::Bool::UniquePtr msg) { if(msg->data) - ROS_INFO("Enabling motors"); + RCLCPP_INFO(this->get_logger(), "Enabling motors"); else - ROS_INFO("Disabling motors"); + RCLCPP_INFO(this->get_logger(), "Disabling motors"); enable_motors_ = msg->data; @@ -215,189 +198,458 @@ void SO3TRPYControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPt controller_.resetIntegrals(); } -void SO3TRPYControlNodelet::corrections_callback(const kr_mav_msgs::Corrections::ConstPtr &msg) +void SO3TRPYControlComponent::corrections_callback(const kr_mav_msgs::msg::Corrections::UniquePtr msg) { corrections_[0] = msg->kf_correction; corrections_[1] = msg->angle_corrections[0]; corrections_[2] = msg->angle_corrections[1]; } - -void SO3TRPYControlNodelet::cfg_callback(kr_mav_controllers::SO3Config &config, uint32_t level) +rcl_interfaces::msg::SetParametersResult SO3TRPYControlComponent::cfg_callback( + std::vector parameters) { - if(level == 0) - { - NODELET_DEBUG_STREAM("Nothing changed. level: " << level); - return; - } - - if(level & (1 << 0)) + rcl_interfaces::msg::SetParametersResult result; + for(auto ¶meter : parameters) { - config_kx_[0] = config.kp_x; - config_kx_[1] = config.kp_y; - config_kx_[2] = config.kp_z; - - config_kv_[0] = config.kd_x; - config_kv_[1] = config.kd_y; - config_kv_[2] = config.kd_z; + const auto &name = parameter.get_name(); - NODELET_INFO("Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", config_kx_[0], - config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); - } - - if(level & (1 << 1)) - { - config_ki_[0] = config.ki_x; - config_ki_[1] = config.ki_y; - config_ki_[2] = config.ki_z; - - config_kib_[0] = config.kib_x; - config_kib_[1] = config.kib_y; - config_kib_[2] = config.kib_z; - - NODELET_INFO("Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", config_ki_[0], - config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); - } - - if(level & (1 << 2)) - { - kR_[0] = config.rot_x; - kR_[1] = config.rot_y; - kR_[2] = config.rot_z; - - kOm_[0] = config.ang_x; - kOm_[1] = config.ang_y; - kOm_[2] = config.ang_z; - - NODELET_INFO("Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", kR_[0], kR_[1], kR_[2], - kOm_[0], kOm_[1], kOm_[2]); - } - - if(level & (1 << 3)) - { - corrections_[0] = config.kf_correction; - corrections_[1] = config.roll_correction; - corrections_[2] = config.pitch_correction; - NODELET_INFO("Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], corrections_[1], - corrections_[2]); - } - - if(level & (1 << 4)) - { - controller_.setMaxIntegral(config.max_pos_int); - controller_.setMaxIntegralBody(config.max_pos_int_b); - controller_.setMaxTiltAngle(config.max_tilt_angle); - - NODELET_INFO("Maxes set to Integral: %2.2g, Integral Body: %2.2g, Tilt Angle (rad): %2.2g", config.max_pos_int, - config.max_pos_int_b, config.max_tilt_angle); + if(name == "kp_x") + { + config_kx_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_y") + { + config_kx_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kp_z") + { + config_kx_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_x") + { + config_kv_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_y") + { + config_kv_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "kd_z") + { + config_kv_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Position Gains set to kp: {%2.3g, %2.3g, %2.3g}, kd: {%2.3g, %2.3g, %2.3g}", + config_kx_[0], config_kx_[1], config_kx_[2], config_kv_[0], config_kv_[1], config_kv_[2]); + } + else if(name == "ki_x") + { + config_ki_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_ki_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_ki_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "kib_x") + { + config_kib_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_y") + { + config_kib_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "ki_z") + { + config_kib_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Integral Gains set to ki: {%2.2g, %2.2g, %2.2g}, kib: {%2.2g, %2.2g, %2.2g}", + config_ki_[0], config_ki_[1], config_ki_[2], config_kib_[0], config_kib_[1], config_kib_[2]); + } + else if(name == "rot_x") + { + kr_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_y") + { + kr_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "rot_z") + { + kr_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_x") + { + kom_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_y") + { + kom_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "ang_z") + { + kom_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Attitude Gains set to kp: {%2.2g, %2.2g, %2.2g}, kd: {%2.2g, %2.2g, %2.2g}", + kr_[0], kr_[1], kr_[2], kom_[0], kom_[1], kom_[2]); + } + else if(name == "kf_correction") + { + corrections_[0] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "roll_correction") + { + corrections_[1] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "pitch_correction") + { + corrections_[2] = parameter.as_double(); + RCLCPP_INFO(this->get_logger(), "Corrections set to kf: %2.2g, roll: %2.2g, pitch: %2.2g", corrections_[0], + corrections_[1], corrections_[2]); + } + else if(name == "max_pos_int") + { + controller_.setMaxIntegral(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "World max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_pos_int_b") + { + controller_.setMaxIntegralBody(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Body max integral set to: %2.2g", parameter.as_double()); + } + else if(name == "max_tilt_angle") + { + controller_.setMaxTiltAngle(parameter.as_double()); + RCLCPP_INFO(this->get_logger(), "Max tilt angle set to: %2.2g", parameter.as_double()); + } + else + { + RCLCPP_WARN_STREAM(this->get_logger(), "kr_mav_controllers dynamic reconfigure called with invalid parameter"); + result.successful = false; + return result; + } } - - NODELET_WARN_STREAM_COND(level != std::numeric_limits::max() && (level >= (1 << 5)), - "kr_mav_controllers dynamic reconfigure called, but with unknown level: " << level); + result.successful = true; + return result; } -void SO3TRPYControlNodelet::onInit() +SO3TRPYControlComponent::SO3TRPYControlComponent(const rclcpp::NodeOptions &options) + : Node("so3_control", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + odom_set_(false), + position_cmd_updated_(false), + position_cmd_init_(false), + des_yaw_(0), + des_yaw_dot_(0), + yaw_int_(0), + enable_motors_(false), + use_external_yaw_(false), + g_(9.81) { - ros::NodeHandle n(getPrivateNodeHandle()); + controller_.resetIntegrals(); + + this->declare_parameter("quadrotor_name", std::string("quadrotor")); + this->declare_parameter("mass", 0.5f); std::string quadrotor_name; - n.param("quadrotor_name", quadrotor_name, std::string("quadrotor")); + this->get_parameter("quadrotor_name", quadrotor_name); frame_id_ = "/" + quadrotor_name; + this->get_parameter("mass", mass_); - float mass; - n.param("mass", mass, 0.5f); - mass_ = mass; controller_.setMass(mass_); controller_.setGravity(g_); - // Dynamic reconfigure struct - kr_mav_controllers::SO3Config config; - - n.param("use_external_yaw", use_external_yaw_, true); - - n.param("gains/pos/x", config_kx_[0], 7.4f); - n.param("gains/pos/y", config_kx_[1], 7.4f); - n.param("gains/pos/z", config_kx_[2], 10.4f); + this->declare_parameter("use_external_yaw", true); + this->get_parameter("use_external_yaw", use_external_yaw_); + + // Dynamic parameter reconfiguration definitions + + // Translation Gains: World Position Gains + rcl_interfaces::msg::ParameterDescriptor desc_kp_x; + rcl_interfaces::msg::FloatingPointRange r_kp_x; + r_kp_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_x.type = 3; // PARAMETER_DOUBLE + desc_kp_x.description = std::string("World x position gain"); + desc_kp_x.floating_point_range = {r_kp_x}; + this->declare_parameter("kp_x", 7.4f, desc_kp_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_y; + rcl_interfaces::msg::FloatingPointRange r_kp_y; + r_kp_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_y.type = 3; // PARAMETER_DOUBLE + desc_kp_y.description = std::string("World y position gain"); + desc_kp_y.floating_point_range = {r_kp_y}; + this->declare_parameter("kp_y", 7.4f, desc_kp_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kp_z; + rcl_interfaces::msg::FloatingPointRange r_kp_z; + r_kp_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kp_z.type = 3; // PARAMETER_DOUBLE + desc_kp_z.description = std::string("World z position gain"); + desc_kp_z.floating_point_range = {r_kp_z}; + this->declare_parameter("kp_z", 10.4f, desc_kp_z); + + // Translation Gains: World Derivative Gains + rcl_interfaces::msg::ParameterDescriptor desc_kd_x; + rcl_interfaces::msg::FloatingPointRange r_kd_x; + r_kd_x.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_x.type = 3; // PARAMETER_DOUBLE + desc_kd_x.description = std::string("World x derivative gain"); + desc_kd_x.floating_point_range = {r_kd_x}; + this->declare_parameter("kd_x", 4.8f, desc_kd_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_y; + rcl_interfaces::msg::FloatingPointRange r_kd_y; + r_kd_y.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_y.type = 3; // PARAMETER_DOUBLE + desc_kd_y.description = std::string("World y derivative gain"); + desc_kd_y.floating_point_range = {r_kd_y}; + this->declare_parameter("kd_y", 4.8f, desc_kd_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kd_z; + rcl_interfaces::msg::FloatingPointRange r_kd_z; + r_kd_z.set__from_value(0.0).set__to_value(20.0).set__step(0); + desc_kd_z.type = 3; // PARAMETER_DOUBLE + desc_kd_z.description = std::string("World z derivative gain"); + desc_kd_z.floating_point_range = {r_kd_z}; + this->declare_parameter("kd_z", 6.0f, desc_kd_z); + + // Integral Gains: World Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_ki_x; + rcl_interfaces::msg::FloatingPointRange r_ki_x; + r_ki_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_x.type = 3; // PARAMETER_DOUBLE + desc_ki_x.description = std::string("World x integral gain"); + desc_ki_x.floating_point_range = {r_ki_x}; + this->declare_parameter("ki_x", 0.0f, desc_ki_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_y; + rcl_interfaces::msg::FloatingPointRange r_ki_y; + r_ki_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_y.type = 3; // PARAMETER_DOUBLE + desc_ki_y.description = std::string("World y integral gain"); + desc_ki_y.floating_point_range = {r_ki_y}; + this->declare_parameter("ki_y", 0.0f, desc_ki_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ki_z; + rcl_interfaces::msg::FloatingPointRange r_ki_z; + r_ki_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_ki_z.type = 3; // PARAMETER_DOUBLE + desc_ki_z.description = std::string("World z integral gain"); + desc_ki_z.floating_point_range = {r_ki_z}; + this->declare_parameter("ki_z", 0.0f, desc_ki_z); + + // Integral Gains: Body Integral Gains + rcl_interfaces::msg::ParameterDescriptor desc_kib_x; + rcl_interfaces::msg::FloatingPointRange r_kib_x; + r_kib_x.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_x.type = 3; // PARAMETER_DOUBLE + desc_kib_x.description = std::string("Body x integral gain"); + desc_kib_x.floating_point_range = {r_kib_x}; + this->declare_parameter("kib_x", 0.0f, desc_kib_x); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_y; + rcl_interfaces::msg::FloatingPointRange r_kib_y; + r_kib_y.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_y.type = 3; // PARAMETER_DOUBLE + desc_kib_y.description = std::string("Body y integral gain"); + desc_kib_y.floating_point_range = {r_kib_y}; + this->declare_parameter("kib_y", 0.0f, desc_kib_y); + + rcl_interfaces::msg::ParameterDescriptor desc_kib_z; + rcl_interfaces::msg::FloatingPointRange r_kib_z; + r_kib_z.set__from_value(0.0).set__to_value(0.2).set__step(0); + desc_kib_z.type = 3; // PARAMETER_DOUBLE + desc_kib_z.description = std::string("Body z integral gain"); + desc_kib_z.floating_point_range = {r_kib_z}; + this->declare_parameter("kib_z", 0.0f, desc_kib_z); + + // Attitude Gains: Rotation Gains + rcl_interfaces::msg::ParameterDescriptor desc_rot_x; + rcl_interfaces::msg::FloatingPointRange r_rot_x; + r_rot_x.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_x.type = 3; // PARAMETER_DOUBLE + desc_rot_x.description = std::string("Rotation x gain"); + desc_rot_x.floating_point_range = {r_rot_x}; + this->declare_parameter("rot_x", 1.5f, desc_rot_x); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_y; + rcl_interfaces::msg::FloatingPointRange r_rot_y; + r_rot_y.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_y.type = 3; // PARAMETER_DOUBLE + desc_rot_y.description = std::string("Rotation y gain"); + desc_rot_y.floating_point_range = {r_rot_y}; + this->declare_parameter("rot_y", 1.5f, desc_rot_y); + + rcl_interfaces::msg::ParameterDescriptor desc_rot_z; + rcl_interfaces::msg::FloatingPointRange r_rot_z; + r_rot_z.set__from_value(0.0).set__to_value(3.00).set__step(0); + desc_rot_z.type = 3; // PARAMETER_DOUBLE + desc_rot_z.description = std::string("Rotation z gain"); + desc_rot_z.floating_point_range = {r_rot_z}; + this->declare_parameter("rot_z", 1.0f, desc_rot_z); + + // Attitude Gains: Angular Gains + rcl_interfaces::msg::ParameterDescriptor desc_ang_x; + rcl_interfaces::msg::FloatingPointRange r_ang_x; + r_ang_x.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_x.type = 3; // PARAMETER_DOUBLE + desc_ang_x.description = std::string("Angular x gain"); + desc_ang_x.floating_point_range = {r_ang_x}; + this->declare_parameter("ang_x", 0.13f, desc_ang_x); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_y; + rcl_interfaces::msg::FloatingPointRange r_ang_y; + r_ang_y.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_y.type = 3; // PARAMETER_DOUBLE + desc_ang_y.description = std::string("Angular y gain"); + desc_ang_y.floating_point_range = {r_ang_y}; + this->declare_parameter("ang_y", 0.13f, desc_ang_y); + + rcl_interfaces::msg::ParameterDescriptor desc_ang_z; + rcl_interfaces::msg::FloatingPointRange r_ang_z; + r_ang_z.set__from_value(0.0).set__to_value(1.00).set__step(0); + desc_ang_z.type = 3; // PARAMETER_DOUBLE + desc_ang_z.description = std::string("Angular z gain"); + desc_ang_z.floating_point_range = {r_ang_z}; + this->declare_parameter("ang_z", 0.10f, desc_ang_z); + + // Low level Corrections + rcl_interfaces::msg::ParameterDescriptor desc_kf_correction; + desc_kf_correction.type = 3; // PARAMETER_DOUBLE + desc_kf_correction.description = std::string("kf"); + this->declare_parameter("kf_correction", 0.0f, desc_kf_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_roll_correction; + desc_roll_correction.type = 3; // PARAMETER_DOUBLE + desc_roll_correction.description = std::string("roll"); + this->declare_parameter("roll_correction", 0.0f, desc_roll_correction); + + rcl_interfaces::msg::ParameterDescriptor desc_pitch_correction; + desc_pitch_correction.type = 3; // PARAMETER_DOUBLE + desc_pitch_correction.description = std::string("pitch"); + this->declare_parameter("pitch_correction", 0.0f, desc_pitch_correction); + + // Misc Parameters + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int; + r_max_pos_int.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int.description = std::string("World max integral"); + desc_max_pos_int.floating_point_range = {r_max_pos_int}; + this->declare_parameter("max_pos_int", 0.5f, desc_max_pos_int); + + rcl_interfaces::msg::ParameterDescriptor desc_max_pos_int_b; + rcl_interfaces::msg::FloatingPointRange r_max_pos_int_b; + r_max_pos_int_b.set__from_value(0.0).set__to_value(4.00).set__step(0); + desc_max_pos_int_b.type = 3; // PARAMETER_DOUBLE + desc_max_pos_int_b.description = std::string("Body max integral"); + desc_max_pos_int_b.floating_point_range = {r_max_pos_int_b}; + this->declare_parameter("max_pos_int_b", 0.5f, desc_max_pos_int_b); + + rcl_interfaces::msg::ParameterDescriptor desc_max_tilt_angle; + rcl_interfaces::msg::FloatingPointRange r_max_tilt_angle; + r_max_tilt_angle.set__from_value(0.0).set__to_value(3.14).set__step(0); + desc_max_tilt_angle.type = 3; // PARAMETER_DOUBLE + desc_max_tilt_angle.description = std::string("Max tilt angle"); + desc_max_tilt_angle.floating_point_range = {r_max_tilt_angle}; + this->declare_parameter("max_tilt_angle", static_cast(M_PI), desc_max_tilt_angle); + + // Getting dynamic parameters + this->get_parameter("kp_x", config_kx_[0]); + this->get_parameter("kp_y", config_kx_[1]); + this->get_parameter("kp_z", config_kx_[2]); kx_[0] = config_kx_[0]; kx_[1] = config_kx_[1]; kx_[2] = config_kx_[2]; - config.kp_x = config_kx_[0]; - config.kp_y = config_kx_[1]; - config.kp_z = config_kx_[2]; - n.param("gains/vel/x", config_kv_[0], 4.8f); - n.param("gains/vel/y", config_kv_[1], 4.8f); - n.param("gains/vel/z", config_kv_[2], 6.0f); + this->get_parameter("kd_x", config_kv_[0]); + this->get_parameter("kd_y", config_kv_[1]); + this->get_parameter("kd_z", config_kv_[2]); kv_[0] = config_kv_[0]; kv_[1] = config_kv_[1]; kv_[2] = config_kv_[2]; - config.kd_x = config_kv_[0]; - config.kd_y = config_kv_[1]; - config.kd_z = config_kv_[2]; - - n.param("gains/ki/x", config_ki_[0], 0.0f); - n.param("gains/ki/y", config_ki_[1], 0.0f); - n.param("gains/ki/z", config_ki_[2], 0.0f); - config.ki_x = config_ki_[0]; - config.ki_y = config_ki_[1]; - config.ki_z = config_ki_[2]; - - n.param("gains/ki/yaw", ki_yaw_, 0.0f); - - n.param("gains/kib/x", config_kib_[0], 0.0f); - n.param("gains/kib/y", config_kib_[1], 0.0f); - n.param("gains/kib/z", config_kib_[2], 0.0f); - config.kib_x = config_kib_[0]; - config.kib_y = config_kib_[1]; - config.kib_z = config_kib_[2]; - - n.param("gains/rot/x", kR_[0], 1.5f); - n.param("gains/rot/y", kR_[1], 1.5f); - n.param("gains/rot/z", kR_[2], 1.0f); - n.param("gains/ang/x", kOm_[0], 0.13f); - n.param("gains/ang/y", kOm_[1], 0.13f); - n.param("gains/ang/z", kOm_[2], 0.1f); - config.rot_x = kR_[0]; - config.rot_y = kR_[1]; - config.rot_z = kR_[2]; - config.ang_x = kOm_[0]; - config.ang_y = kOm_[1]; - config.ang_z = kOm_[2]; - - n.param("corrections/kf", corrections_[0], 0.0f); - n.param("corrections/r", corrections_[1], 0.0f); - n.param("corrections/p", corrections_[2], 0.0f); - config.kf_correction = corrections_[0]; - config.roll_correction = corrections_[1]; - config.pitch_correction = corrections_[2]; + + this->get_parameter("ki_x", config_ki_[0]); + this->get_parameter("ki_y", config_ki_[1]); + this->get_parameter("ki_z", config_ki_[2]); + + this->get_parameter("kib_x", config_kib_[0]); + this->get_parameter("kib_y", config_kib_[1]); + this->get_parameter("kib_z", config_kib_[2]); + + this->get_parameter("rot_x", kr_[0]); + this->get_parameter("rot_y", kr_[1]); + this->get_parameter("rot_z", kr_[2]); + + this->get_parameter("ang_x", kom_[0]); + this->get_parameter("ang_y", kom_[1]); + this->get_parameter("ang_z", kom_[2]); + + this->get_parameter("kf_correction", corrections_[0]); + this->get_parameter("roll_correction", corrections_[1]); + this->get_parameter("pitch_correction", corrections_[2]); float max_pos_int, max_pos_int_b; - n.param("max_pos_int", max_pos_int, 0.5f); - n.param("mas_pos_int_b", max_pos_int_b, 0.5f); + this->get_parameter("max_pos_int", max_pos_int); + this->get_parameter("max_pos_int_b", max_pos_int_b); controller_.setMaxIntegral(max_pos_int); controller_.setMaxIntegralBody(max_pos_int_b); - config.max_pos_int = max_pos_int; - config.max_pos_int_b = max_pos_int_b; float max_tilt_angle; - n.param("max_tilt_angle", max_tilt_angle, static_cast(M_PI)); + this->get_parameter("max_tilt_angle", max_tilt_angle); controller_.setMaxTiltAngle(max_tilt_angle); - config.max_tilt_angle = max_tilt_angle; - - // Initialize dynamic reconfigure - reconfigure_server_ = boost::make_shared(config_mutex_, n); - reconfigure_server_->updateConfig(config); - reconfigure_server_->setCallback(boost::bind(&SO3TRPYControlNodelet::cfg_callback, this, _1, _2)); - - trpy_command_pub_ = n.advertise("trpy_cmd", 10); - - odom_sub_ = n.subscribe("odom", 10, &SO3TRPYControlNodelet::odom_callback, this, ros::TransportHints().tcpNoDelay()); - position_cmd_sub_ = n.subscribe("position_cmd", 10, &SO3TRPYControlNodelet::position_cmd_callback, this, - ros::TransportHints().tcpNoDelay()); - enable_motors_sub_ = n.subscribe("motors", 2, &SO3TRPYControlNodelet::enable_motors_callback, this, - ros::TransportHints().tcpNoDelay()); - corrections_sub_ = n.subscribe("corrections", 10, &SO3TRPYControlNodelet::corrections_callback, this, - ros::TransportHints().tcpNoDelay()); + + // Initialize dynamic reconfigure callback + reconfigure_handle_ = this->add_on_set_parameters_callback( + std::bind(&SO3TRPYControlComponent::cfg_callback, this, std::placeholders::_1)); + + trpy_command_pub_ = this->create_publisher("~/trpy_cmd", 10); + + // Setting QoS profile to get equivalent performance to ros::TransportHints().tcpNoDelay() + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos1 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + auto qos2 = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 2), qos_profile); + + odom_sub_ = this->create_subscription( + "~/odom", qos1, std::bind(&SO3TRPYControlComponent::odom_callback, this, std::placeholders::_1)); + position_cmd_sub_ = this->create_subscription( + "~/position_cmd", qos1, std::bind(&SO3TRPYControlComponent::position_cmd_callback, this, std::placeholders::_1)); + enable_motors_sub_ = this->create_subscription( + "~/motors", qos2, std::bind(&SO3TRPYControlComponent::enable_motors_callback, this, std::placeholders::_1)); + corrections_sub_ = this->create_subscription( + "~/corrections", qos1, std::bind(&SO3TRPYControlComponent::corrections_callback, this, std::placeholders::_1)); } -#include -PLUGINLIB_EXPORT_CLASS(SO3TRPYControlNodelet, nodelet::Nodelet); +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3TRPYControlComponent) diff --git a/kr_mav_controllers/test/launch/so3_control_component_test.test.py b/kr_mav_controllers/test/launch/so3_control_component_test.test.py new file mode 100755 index 00000000..54c5ab8c --- /dev/null +++ b/kr_mav_controllers/test/launch/so3_control_component_test.test.py @@ -0,0 +1,91 @@ +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node +import launch_testing +import launch_testing.actions +import launch +import unittest +import os + +def generate_test_description(): + + component_under_test = ComposableNodeContainer( + name="so3_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions= [ + ComposableNode( + package="kr_mav_controllers", + plugin="SO3ControlComponent", + name="SO3ControlComponent", + remappings=[ + ("~/odom", "odom"), + ("~/position_cmd", "position_cmd"), + ("~/motors", "motors"), + ("~/corrections", "corrections"), + ("~/so3_cmd", "so3_cmd") + ] + ) + ], + output="screen" + ) + + so3_control_component_test = Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "so3_control_component_test", + ] + ), + output="screen", + ) + + # test_executable_path = os.path.join( + # get_package_share_directory("kr_mav_controllers"), + # 'test', # Executables are usually installed to lib/ in ROS 2 + # "so3_control_component_test.cpp" + # ) + # so3_control_component_test = Node( + # package="kr_mav_controllers", + # executable="so3_control_component_test", + # name="so3_control_tester", + # output="screen" + # ) + + + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + component_under_test, + so3_control_component_test, + launch_testing.actions.ReadyToTest(), + ]), {'component_under_test': component_under_test, + 'so3_control_component_test': so3_control_component_test} + + # return LaunchDescription([ + # component_under_test, + # so3_control_component_test, + # launch_testing.actions.ReadyToTest() + # ]), {'component_under_test': component_under_test, + # 'so3_control_component_test': so3_control_component_test} + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, proc_info, so3_control_component_test): + proc_info.assertWaitForShutdown(so3_control_component_test, timeout=1000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes + def test_gtest_pass(self, proc_info, so3_control_component_test): + launch_testing.asserts.assertExitCodes( + proc_info, process=so3_control_component_test + ) + diff --git a/kr_mav_controllers/test/so3_control_component_test.cpp b/kr_mav_controllers/test/so3_control_component_test.cpp new file mode 100644 index 00000000..13450ee1 --- /dev/null +++ b/kr_mav_controllers/test/so3_control_component_test.cpp @@ -0,0 +1,241 @@ +#include + +#include +#include + +#include "kr_mav_controllers/so3_control_tester.hpp" + +class SO3ControlComponentTest : public testing::Test +{ + public: + SO3ControlComponentTest() + : tester(std::make_shared()), + executor(std::make_shared()) + { + } + + std::shared_ptr tester; + std::thread spin_thread; + std::shared_ptr executor; + + // Spinning the node in a different thread + void SetUp() override + { + executor->add_node(tester); + spin_thread = std::thread([this]() { executor->spin(); }); + rclcpp::sleep_for(1s); + } + + // Cancel spinning of node + void TearDown() override + { + executor->cancel(); + spin_thread.join(); + } +}; + +/* + * @brief Test1: test if SO3Command is received without enabling any motors or odom data or position command + * Expected behavior: so3_command_received_ = false + * Also checking if the component is active + */ +TEST_F(SO3ControlComponentTest, Test1) +{ + ASSERT_TRUE(tester->is_so3_cmd_publisher_active()); // checking if nodelet is active + std::cout << "Performing Test1!\n"; + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } +} + +/* + * @brief Test2: publish position command and check so3 command + * Expected behavior: so3_command_received_ = false + */ +TEST_F(SO3ControlComponentTest, Test2) +{ + std::cout << "Performing Test2!\n"; + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test3: enable motors and publish position command. check so3 command + * Expected behavior: so3_command_received_ = false + */ +TEST_F(SO3ControlComponentTest, Test3) +{ + tester->publish_enable_motors(true); + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_FALSE(tester->so3_command_received_); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test4: enable motors, publish odom, publish position command. check so3 command + * Expected behavior: so3_command_received_ = true + */ +TEST_F(SO3ControlComponentTest, Test4) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->publish_odom_msg(0); + rclcpp::sleep_for(1s); + tester->publish_single_position_command(); + rclcpp::sleep_for(1s); + Test4Data ref; + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_EQ(tester->so3_cmd_->force.x, ref.force_x); + EXPECT_EQ(tester->so3_cmd_->force.y, ref.force_y); + EXPECT_EQ(tester->so3_cmd_->force.z, ref.force_z); + EXPECT_EQ(tester->so3_cmd_->orientation.x, ref.orientation_x); + EXPECT_EQ(tester->so3_cmd_->orientation.y, ref.orientation_y); + EXPECT_EQ(tester->so3_cmd_->orientation.z, ref.orientation_z); + EXPECT_EQ(tester->so3_cmd_->orientation.w, ref.orientation_w); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y); + EXPECT_EQ(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z); + EXPECT_EQ(tester->so3_cmd_->aux.current_yaw, ref.current_yaw); + EXPECT_EQ(tester->so3_cmd_->aux.kf_correction, ref.kf_correction); + EXPECT_EQ(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0]); + EXPECT_EQ(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1]); + } + tester->reset_so3_cmd_pointer(); +} + +/* + * @brief Test5: enable motors, publish odom, publish position commands (11 cmds). Check so3 command + */ +TEST_F(SO3ControlComponentTest, Test5) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(1, 1, 2, 0, 0.1); + tester->publish_odom_msg(0); + rclcpp::sleep_for(1s); + Test5Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +/* + * @brief Test6: enable motors, publish odom, publish position commands (11 cmds). Check so3 command + */ +TEST_F(SO3ControlComponentTest, Test6) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(2, 3, 5, 7, 0.2); + tester->publish_odom_msg(1); + rclcpp::sleep_for(1s); + Test6Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +/* + * @brief Test7: enable motors, publish corrections, publish odom, publish position commands (11 cmds). Check so3 + * command + */ +TEST_F(SO3ControlComponentTest, Test7) +{ + tester->publish_enable_motors(true); + tester->populate_odom_msgs(); + tester->populate_position_cmd_vector(4, 2, 1, 56, 0.5); + float kf_corr = 1.0f; + float ang_corr[2] = {0.2f, 0.3f}; + tester->publish_corrections(kf_corr, ang_corr); + rclcpp::sleep_for(1s); + tester->publish_odom_msg(2); + rclcpp::sleep_for(1s); + Test7Data ref; + for(int i = 0; i < 11; i++) + { + tester->publish_position_command(i); + rclcpp::sleep_for(1s); + { + std::lock_guard lock(tester->mutex); + EXPECT_TRUE(tester->so3_command_received_); + EXPECT_NEAR(tester->so3_cmd_->force.x, ref.force_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.y, ref.force_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->force.z, ref.force_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); + EXPECT_NEAR(tester->so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); + } + tester->reset_so3_cmd_pointer(); + } +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + auto result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/kr_mav_controllers/test/so3_control_nodelet.test b/kr_mav_controllers/test/so3_control_nodelet.test deleted file mode 100644 index 3977294e..00000000 --- a/kr_mav_controllers/test/so3_control_nodelet.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/kr_mav_controllers/test/so3_control_nodelet_test.cpp b/kr_mav_controllers/test/so3_control_nodelet_test.cpp deleted file mode 100644 index 1df5f62d..00000000 --- a/kr_mav_controllers/test/so3_control_nodelet_test.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include "kr_mav_controllers/so3_control_tester.hpp" - -/* - * @brief Test1: test if SO3Command is received without enabling any motors or odom data or position command - * Expected behavior: so3_command_received_ = false - * Also checking if the nodelet is active - */ -TEST(SO3ControlNodeletTest, Test1) -{ - std::cout << "Performing Test1!\n"; - SO3ControlTester tester; - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - ASSERT_TRUE(tester.is_so3_cmd_publisher_active()); // checking if nodelet is active -} - -/* - * @brief Test2: publish position command and check so3 command - * Expected behavior: so3_command_received_ = false - */ -TEST(SO3ControlNodeletTest, Test2) -{ - SO3ControlTester tester; - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - tester.reset_so3_cmd_pointer(); -} - -/* - * @brief Test3: enable motors and publish position command. check so3 command - * Expected behavior: so3_command_received_ = false - */ -TEST(SO3ControlNodeletTest, Test3) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_FALSE(tester.so3_command_received_); - } - tester.reset_so3_cmd_pointer(); -} - -/* - * @brief Test4: enable motors, publish odom, publish position command. check so3 command - * Expected behavior: so3_command_received_ = true - */ -TEST(SO3ControlNodeletTest, Test4) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.publish_odom_msg(0); - ros::Duration(1.0).sleep(); - tester.publish_single_position_command(); - ros::Duration(1.0).sleep(); - Test4Data ref; - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_EQ(tester.so3_cmd_->force.x, ref.force_x); - EXPECT_EQ(tester.so3_cmd_->force.y, ref.force_y); - EXPECT_EQ(tester.so3_cmd_->force.z, ref.force_z); - EXPECT_EQ(tester.so3_cmd_->orientation.x, ref.orientation_x); - EXPECT_EQ(tester.so3_cmd_->orientation.y, ref.orientation_y); - EXPECT_EQ(tester.so3_cmd_->orientation.z, ref.orientation_z); - EXPECT_EQ(tester.so3_cmd_->orientation.w, ref.orientation_w); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y); - EXPECT_EQ(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z); - EXPECT_EQ(tester.so3_cmd_->aux.current_yaw, ref.current_yaw); - EXPECT_EQ(tester.so3_cmd_->aux.kf_correction, ref.kf_correction); - EXPECT_EQ(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0]); - EXPECT_EQ(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1]); - } - tester.reset_so3_cmd_pointer(); -} - -/* -* @brief Test5: enable motors, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test5) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(1, 1, 2, 0, 0.1); - tester.publish_odom_msg(0); - ros::Duration(1.0).sleep(); - Test5Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -/* -* @brief Test6: enable motors, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test6) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(2, 3, 5, 7, 0.2); - tester.publish_odom_msg(1); - ros::Duration(1.0).sleep(); - Test6Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -/* -* @brief Test7: enable motors, publish corrections, publish odom, publish position commands (11 cmds). Check so3 command -*/ -TEST(SO3ControlNodeletTest, Test7) -{ - SO3ControlTester tester; - tester.publish_enable_motors(true); - tester.populate_odom_msgs(); - tester.populate_position_cmd_vector(4, 2, 1, 56, 0.5); - float kf_corr = 1.0f; - float ang_corr[2] = {0.2f, 0.3f}; - tester.publish_corrections(kf_corr, ang_corr); - ros::Duration(1.0).sleep(); - tester.publish_odom_msg(2); - ros::Duration(1.0).sleep(); - Test7Data ref; - for(int i = 0; i < 11; i++) - { - tester.publish_position_command(i); - ros::Duration(1.0).sleep(); - { - std::lock_guard lock(tester.mutex); - EXPECT_TRUE(tester.so3_command_received_); - EXPECT_NEAR(tester.so3_cmd_->force.x, ref.force_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.y, ref.force_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->force.z, ref.force_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.x, ref.orientation_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.y, ref.orientation_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.z, ref.orientation_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->orientation.w, ref.orientation_w[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.x, ref.angular_velocity_x[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.y, ref.angular_velocity_y[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->angular_velocity.z, ref.angular_velocity_z[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.current_yaw, ref.current_yaw[i], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.kf_correction, ref.kf_correction, 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[0], ref.angle_corrections[0], 1e-4); - EXPECT_NEAR(tester.so3_cmd_->aux.angle_corrections[1], ref.angle_corrections[1], 1e-4); - } - tester.reset_so3_cmd_pointer(); - } -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "so3_control_nodelet_tester"); - testing::InitGoogleTest(&argc, argv); - - std::thread t([] { - while (ros::ok()) - ros::spin(); - }); - - auto res = RUN_ALL_TESTS(); - - ros::shutdown(); - - t.join(); - - return res; -} From 64981e40c72cbdb65c7d6acc980c85170dcc46f3 Mon Sep 17 00:00:00 2001 From: Fernando Cladera Date: Wed, 13 Mar 2024 18:35:00 -0400 Subject: [PATCH 2/5] Disable failing cpplint tests that conflict with coding style --- .github/workflows/cpplint.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/cpplint.yml b/.github/workflows/cpplint.yml index 0bea07fd..f6d19e5d 100644 --- a/.github/workflows/cpplint.yml +++ b/.github/workflows/cpplint.yml @@ -15,6 +15,8 @@ jobs: ,-whitespace/braces\ ,-whitespace/parens\ ,-whitespace/newline\ + ,-build/header_guard\ + ,-readability/todo\ ,-build/c++11" # -readability/braces\ # ,-whitespace/comments\ From cdde203d5a8943614f6e348fa2f286ceab6482ac Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 19:04:09 -0400 Subject: [PATCH 3/5] cpplint compliant changes --- .../kr_mav_controllers/so3_control_tester.hpp | 7 +++--- .../src/pid_control_component.cpp | 2 +- .../src/so3_control_component.cpp | 2 +- kr_mav_controllers/src/so3_trpy_control.cpp | 2 +- .../test/so3_control_component_test.cpp | 24 +++++++++---------- 5 files changed, 19 insertions(+), 18 deletions(-) diff --git a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp index 623d5071..b5358654 100644 --- a/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp +++ b/kr_mav_controllers/include/kr_mav_controllers/so3_control_tester.hpp @@ -1,3 +1,6 @@ +#include +#include + #include "kr_mav_msgs/msg/corrections.hpp" #include "kr_mav_msgs/msg/position_command.hpp" #include "kr_mav_msgs/msg/so3_command.hpp" @@ -5,8 +8,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" -using namespace std::chrono_literals; - class SO3ControlTester : public rclcpp::Node { public: @@ -58,7 +59,7 @@ bool SO3ControlTester::is_so3_cmd_publisher_active() flag = true; else { - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); if(so3_cmd_sub_->get_publisher_count() > 0) flag = true; else diff --git a/kr_mav_controllers/src/pid_control_component.cpp b/kr_mav_controllers/src/pid_control_component.cpp index 35381fa3..bb6a0351 100644 --- a/kr_mav_controllers/src/pid_control_component.cpp +++ b/kr_mav_controllers/src/pid_control_component.cpp @@ -14,7 +14,7 @@ class PIDControlComponent : public rclcpp::Node { public: - PIDControlComponent(const rclcpp::NodeOptions &options); + explicit PIDControlComponent(const rclcpp::NodeOptions &options); private: void publishTRPYCommand(void); diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 823a8946..9a80d54b 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -14,7 +14,7 @@ class SO3ControlComponent : public rclcpp::Node { public: - SO3ControlComponent(const rclcpp::NodeOptions &options); + explicit SO3ControlComponent(const rclcpp::NodeOptions &options); private: void publishSO3Command(); diff --git a/kr_mav_controllers/src/so3_trpy_control.cpp b/kr_mav_controllers/src/so3_trpy_control.cpp index 3c4b750f..d212ee63 100644 --- a/kr_mav_controllers/src/so3_trpy_control.cpp +++ b/kr_mav_controllers/src/so3_trpy_control.cpp @@ -13,7 +13,7 @@ class SO3TRPYControlComponent : public rclcpp::Node { public: - SO3TRPYControlComponent(const rclcpp::NodeOptions &options); + explicit SO3TRPYControlComponent(const rclcpp::NodeOptions &options); private: void publishCommand(); diff --git a/kr_mav_controllers/test/so3_control_component_test.cpp b/kr_mav_controllers/test/so3_control_component_test.cpp index 13450ee1..a7928597 100644 --- a/kr_mav_controllers/test/so3_control_component_test.cpp +++ b/kr_mav_controllers/test/so3_control_component_test.cpp @@ -23,7 +23,7 @@ class SO3ControlComponentTest : public testing::Test { executor->add_node(tester); spin_thread = std::thread([this]() { executor->spin(); }); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); } // Cancel spinning of node @@ -57,7 +57,7 @@ TEST_F(SO3ControlComponentTest, Test2) { std::cout << "Performing Test2!\n"; tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_FALSE(tester->so3_command_received_); @@ -73,7 +73,7 @@ TEST_F(SO3ControlComponentTest, Test3) { tester->publish_enable_motors(true); tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_FALSE(tester->so3_command_received_); @@ -90,9 +90,9 @@ TEST_F(SO3ControlComponentTest, Test4) tester->publish_enable_motors(true); tester->populate_odom_msgs(); tester->publish_odom_msg(0); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); tester->publish_single_position_command(); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test4Data ref; { std::lock_guard lock(tester->mutex); @@ -124,12 +124,12 @@ TEST_F(SO3ControlComponentTest, Test5) tester->populate_odom_msgs(); tester->populate_position_cmd_vector(1, 1, 2, 0, 0.1); tester->publish_odom_msg(0); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test5Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); @@ -161,12 +161,12 @@ TEST_F(SO3ControlComponentTest, Test6) tester->populate_odom_msgs(); tester->populate_position_cmd_vector(2, 3, 5, 7, 0.2); tester->publish_odom_msg(1); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test6Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); @@ -201,14 +201,14 @@ TEST_F(SO3ControlComponentTest, Test7) float kf_corr = 1.0f; float ang_corr[2] = {0.2f, 0.3f}; tester->publish_corrections(kf_corr, ang_corr); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); tester->publish_odom_msg(2); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); Test7Data ref; for(int i = 0; i < 11; i++) { tester->publish_position_command(i); - rclcpp::sleep_for(1s); + rclcpp::sleep_for(std::chrono::seconds(1)); { std::lock_guard lock(tester->mutex); EXPECT_TRUE(tester->so3_command_received_); From 2aa814eb1a8fb8852d184935295a0d3e6d3f4943 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Wed, 13 Mar 2024 21:21:07 -0400 Subject: [PATCH 4/5] refined comments --- .../launch/so3_control_component_test.test.py | 25 ++----------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/kr_mav_controllers/test/launch/so3_control_component_test.test.py b/kr_mav_controllers/test/launch/so3_control_component_test.test.py index 54c5ab8c..d3382f0a 100755 --- a/kr_mav_controllers/test/launch/so3_control_component_test.test.py +++ b/kr_mav_controllers/test/launch/so3_control_component_test.test.py @@ -7,10 +7,10 @@ import launch_testing.actions import launch import unittest -import os def generate_test_description(): + # initialize component component_under_test = ComposableNodeContainer( name="so3_container", namespace="", @@ -33,6 +33,7 @@ def generate_test_description(): output="screen" ) + # initialize test node so3_control_component_test = Node( executable=launch.substitutions.PathJoinSubstitution( [ @@ -43,19 +44,6 @@ def generate_test_description(): output="screen", ) - # test_executable_path = os.path.join( - # get_package_share_directory("kr_mav_controllers"), - # 'test', # Executables are usually installed to lib/ in ROS 2 - # "so3_control_component_test.cpp" - # ) - # so3_control_component_test = Node( - # package="kr_mav_controllers", - # executable="so3_control_component_test", - # name="so3_control_tester", - # output="screen" - # ) - - return LaunchDescription([ launch.actions.DeclareLaunchArgument( name="test_binary_dir", @@ -68,19 +56,11 @@ def generate_test_description(): ]), {'component_under_test': component_under_test, 'so3_control_component_test': so3_control_component_test} - # return LaunchDescription([ - # component_under_test, - # so3_control_component_test, - # launch_testing.actions.ReadyToTest() - # ]), {'component_under_test': component_under_test, - # 'so3_control_component_test': so3_control_component_test} - class TestGTestWaitForCompletion(unittest.TestCase): # Waits for test to complete, then waits a bit to make sure result files are generated def test_gtest_run_complete(self, proc_info, so3_control_component_test): proc_info.assertWaitForShutdown(so3_control_component_test, timeout=1000.0) - @launch_testing.post_shutdown_test() class TestGTestProcessPostShutdown(unittest.TestCase): # Checks if the test has been completed with acceptable exit codes @@ -88,4 +68,3 @@ def test_gtest_pass(self, proc_info, so3_control_component_test): launch_testing.asserts.assertExitCodes( proc_info, process=so3_control_component_test ) - From e5fbc6ca583c2bd494018424591f1ee46927d292 Mon Sep 17 00:00:00 2001 From: pranavpshah Date: Fri, 15 Mar 2024 15:40:32 -0400 Subject: [PATCH 5/5] removed colcon test block --- kr_mesh_visualization/CMakeLists.txt | 12 ------------ trackers/kr_tracker_msgs/CMakeLists.txt | 11 ----------- 2 files changed, 23 deletions(-) diff --git a/kr_mesh_visualization/CMakeLists.txt b/kr_mesh_visualization/CMakeLists.txt index 0bb9f529..7120fb56 100644 --- a/kr_mesh_visualization/CMakeLists.txt +++ b/kr_mesh_visualization/CMakeLists.txt @@ -26,16 +26,4 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/trackers/kr_tracker_msgs/CMakeLists.txt b/trackers/kr_tracker_msgs/CMakeLists.txt index d8eb6bdf..3e3f9158 100644 --- a/trackers/kr_tracker_msgs/CMakeLists.txt +++ b/trackers/kr_tracker_msgs/CMakeLists.txt @@ -38,17 +38,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES std_msgs builtin_interfaces geometry_msgs ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_export_dependencies(rosidl_default_runtime) ament_package()