From 30e31b45a567f6d84737d55bca06a36301c91f7e Mon Sep 17 00:00:00 2001 From: Dexter Date: Mon, 4 May 2026 23:22:02 +0000 Subject: [PATCH 1/6] Added circle and lissajous trackers --- .../config/neurofly.yaml | 25 +- .../config/tracker_params.yaml | 25 - .../config/trackers.yaml | 5 +- kr_mav_launch/config/quadrotor_trackers.yaml | 7 +- .../kr_mav_manager/mav_manager_services.hpp | 2 +- kr_mav_manager/src/manager.cpp | 51 +- rqt_mav_manager/rqt_mav_manager/__init__.py | 215 +++++- trackers/kr_trackers/CMakeLists.txt | 6 +- .../include/kr_trackers/lissajous_generator.h | 48 +- trackers/kr_trackers/plugins_description.xml | 15 + .../kr_trackers/src/circle_tracker_server.cpp | 675 +++++++----------- .../src/lissajous_adder_server.cpp | 329 +++++---- .../kr_trackers/src/lissajous_generator.cpp | 78 +- .../src/lissajous_tracker_server.cpp | 299 ++++---- .../config/trackers_manager.yaml | 5 +- 15 files changed, 991 insertions(+), 794 deletions(-) delete mode 100644 interfaces/kr_betaflight_interface/config/tracker_params.yaml diff --git a/interfaces/kr_betaflight_interface/config/neurofly.yaml b/interfaces/kr_betaflight_interface/config/neurofly.yaml index 6e884262..f0649f15 100644 --- a/interfaces/kr_betaflight_interface/config/neurofly.yaml +++ b/interfaces/kr_betaflight_interface/config/neurofly.yaml @@ -3,21 +3,22 @@ protocol_type: crsf # crsf, sbus control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' # set in the 'flight_controller'!) rc_timeout: 0.1 # [s] +mass: 0.68 # [kg] disable_thrust_mapping: true use_body_rates: false -## fpvcycle 2206 motors 4S -# thrust_vs_rpm_cof_a_: 1.755e-06 # gf -# thrust_vs_rpm_cof_b_: -6.521e-03 # gf -# thrust_vs_rpm_cof_c_: 1.388e+01 # gf -thrust_vs_rpm_cof_a_: 1.72106707e-8 # N -thrust_vs_rpm_cof_b_: -0.00006394916 # N -thrust_vs_rpm_cof_c_: 0.136116302 # N -rpm_vs_throttle_linear_coeff_a_: 2.89518953e+01 -rpm_vs_throttle_linear_coeff_b_: -3.04658581e+04 -rpm_vs_throttle_quadratic_coeff_a_: -2.13455563e-02 -rpm_vs_throttle_quadratic_coeff_b_: 9.61348839e+01 -rpm_vs_throttle_quadratic_coeff_c_: -8.20339135e+04 +## hyperlite 1804 motors 4S +# thrust_vs_rpm_cof_a_: 1.338e-06 # gf +# thrust_vs_rpm_cof_b_: -4.472e-3 # gf +# thrust_vs_rpm_cof_c_: 8.051 # gf +thrust_vs_rpm_cof_a_: 1.31212977e-08 # N +thrust_vs_rpm_cof_b_: -4.38553e-05 # N +thrust_vs_rpm_cof_c_: 7.89533392e-02 # N +rpm_vs_throttle_linear_coeff_a_: 17.6 +rpm_vs_throttle_linear_coeff_b_: -15875.0 +rpm_vs_throttle_quadratic_coeff_a_: -30169.81 +rpm_vs_throttle_quadratic_coeff_b_: 35.43775 +rpm_vs_throttle_quadratic_coeff_c_: -0.004962819 # Maximum values for body rates and roll and pitch angles as they are set # on the Flight Controller. The max roll an pitch angles are only active diff --git a/interfaces/kr_betaflight_interface/config/tracker_params.yaml b/interfaces/kr_betaflight_interface/config/tracker_params.yaml deleted file mode 100644 index af1d71a1..00000000 --- a/interfaces/kr_betaflight_interface/config/tracker_params.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# This should contain tracker parameters - -line_tracker_distance: - default_v_des: 0.2 - default_a_des: 0.2 - epsilon: 0.1 - -line_tracker_min_jerk: - default_v_des: 1.0 - default_a_des: 0.5 - default_yaw_v_des: 0.4 - default_yaw_a_des: 0.3 - -trajectory_tracker: - max_vel_des: 2.0 - max_acc_des: 2.0 - -velocity_tracker: - timeout: 0.5 - -lissajous_tracker: - frame_id: odom - -lissajous_adder: - frame_id: odom \ No newline at end of file diff --git a/interfaces/kr_betaflight_interface/config/trackers.yaml b/interfaces/kr_betaflight_interface/config/trackers.yaml index f8d96393..9f3477b5 100644 --- a/interfaces/kr_betaflight_interface/config/trackers.yaml +++ b/interfaces/kr_betaflight_interface/config/trackers.yaml @@ -1,6 +1,7 @@ /neurofly1/trackers_manager: ros__parameters: - trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker", "CircleTracker", "LissajousTracker", "LissajousAdder"] + circle_tracker/ramp_up_time: 2.0 line_tracker_distance/default_v_des: 0.2 line_tracker_distance/default_a_des: 0.2 line_tracker_distance/epsilon: 0.1 @@ -8,3 +9,5 @@ line_tracker_min_jerk/default_a_des: 0.5 line_tracker_min_jerk/default_yaw_v_des: 0.4 line_tracker_min_jerk/default_yaw_a_des: 0.3 + lissajous_tracker/frame_id: odom + lissajous_adder/frame_id: odom diff --git a/kr_mav_launch/config/quadrotor_trackers.yaml b/kr_mav_launch/config/quadrotor_trackers.yaml index ffb9eb62..7c9c16af 100644 --- a/kr_mav_launch/config/quadrotor_trackers.yaml +++ b/kr_mav_launch/config/quadrotor_trackers.yaml @@ -1,11 +1,14 @@ /**/trackers_manager: ros__parameters: # Corrected trackers list: each tracker must be a separate string element - trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker", "CircleTracker", "LissajousTracker", "LissajousAdder"] + circle_tracker/ramp_up_time: 2.0 line_tracker_distance/default_v_des: 0.2 line_tracker_distance/default_a_des: 0.2 line_tracker_distance/epsilon: 0.1 line_tracker_min_jerk/default_v_des: 1.0 line_tracker_min_jerk/default_a_des: 0.5 line_tracker_min_jerk/default_yaw_v_des: 0.4 - line_tracker_min_jerk/default_yaw_a_des: 0.3 \ No newline at end of file + line_tracker_min_jerk/default_yaw_a_des: 0.3 + lissajous_tracker/frame_id: odom + lissajous_adder/frame_id: odom \ No newline at end of file diff --git a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp index 633fcb70..cc865c55 100644 --- a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp +++ b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp @@ -109,7 +109,7 @@ class MAVManagerServices const kr_mav_manager::srv::Lissajous::Response::SharedPtr res) { res->success = - mav->lissajous(req->x_amp, req->y_amp, req->z_amp, req->y_amp, req->x_num_periods, req->y_num_periods, + mav->lissajous(req->x_amp, req->y_amp, req->z_amp, req->yaw_amp, req->x_num_periods, req->y_num_periods, req->z_num_periods, req->yaw_num_periods, req->period, req->num_cycles, req->ramp_time); res->message = "Lissajous motion"; if(res->success) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 145afc66..f9c3444f 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -511,9 +511,26 @@ bool MAVManager::circle(float Ax, float Ay, float T, float duration) auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::circle_tracker_done_callback, this, _1); + // Ensure the tracker transition only happens after the goal is accepted by the tracker server. + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "CircleTracker goal was rejected by the server"); + return; + } + + // Goal accepted; request transition to CircleTracker + if (!this->transition(circle_tracker_str)) + { + RCLCPP_WARN(this->get_logger(), "Transition to CircleTracker failed after goal acceptance"); + } + }; + circle_tracker_client_->async_send_goal(goal, options); - return this->transition(circle_tracker_str); + // Goal successfully sent (async). Actual transition will occur in the goal response callback. + return true; } bool MAVManager::lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, float x_num_periods, @@ -542,9 +559,23 @@ bool MAVManager::lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::lissajous_tracker_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LissajousTracker goal was rejected by the server"); + return; + } + + if (!this->transition(lissajous_tracker_str)) + { + RCLCPP_WARN(this->get_logger(), "Transition to LissajousTracker failed after goal acceptance"); + } + }; + lissajous_tracker_client_->async_send_goal(goal, options); - return this->transition(lissajous_tracker_str); + return true; } bool MAVManager::compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[2], float yaw_amp[2], @@ -584,9 +615,23 @@ bool MAVManager::compound_lissajous(float x_amp[2], float y_amp[2], float z_amp[ auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::lissajous_adder_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LissajousAdder goal was rejected by the server"); + return; + } + + if (!this->transition(lissajous_adder_str)) + { + RCLCPP_WARN(this->get_logger(), "Transition to LissajousAdder failed after goal acceptance"); + } + }; + lissajous_adder_client_->async_send_goal(goal, options); - return this->transition(lissajous_adder_str); + return true; } // World Velocity Commands diff --git a/rqt_mav_manager/rqt_mav_manager/__init__.py b/rqt_mav_manager/rqt_mav_manager/__init__.py index 2b26f460..a73877f5 100644 --- a/rqt_mav_manager/rqt_mav_manager/__init__.py +++ b/rqt_mav_manager/rqt_mav_manager/__init__.py @@ -1,11 +1,19 @@ #!/usr/bin/env python3 -from cairo import STATUS_INVALID_STATUS import rclpy import os from python_qt_binding import loadUi -from PyQt5.QtWidgets import QWidget +from PyQt5.QtWidgets import ( + QWidget, + QVBoxLayout, + QTabWidget, + QGridLayout, + QGroupBox, + QLabel, + QDoubleSpinBox, + QPushButton, +) from rqt_gui_py.plugin import Plugin from ament_index_python import get_resource @@ -30,9 +38,26 @@ def __init__(self, context): loadUi(ui_file, self._widget) self._widget.setObjectName('MAVManagerWidget') + self._tab_widget = QTabWidget() + self._tab_widget.addTab(self._widget, 'MAV Control') + + self._traj_widget = QWidget() + self._traj_widget.setObjectName('TrajectoryTab') + self._build_trajectory_tab(self._traj_widget) + self._tab_widget.addTab(self._traj_widget, 'Trajectories') + + self._container_widget = QWidget() + self._container_widget.setObjectName('MAVManagerContainerWidget') + container_layout = QVBoxLayout(self._container_widget) + container_layout.setContentsMargins(0, 0, 0, 0) + container_layout.addWidget(self._tab_widget) + if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - self._context.add_widget(self._widget) + self._container_widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) + else: + self._container_widget.setWindowTitle(self._widget.windowTitle()) + + self._context.add_widget(self._container_widget) self._widget.robot_name_line_edit.textChanged.connect(self._on_robot_name_changed) self._widget.node_name_line_edit.textChanged.connect(self._on_node_name_changed) @@ -49,6 +74,155 @@ def __init__(self, context): self._widget.takeoff_push_button.pressed.connect(self._on_takeoff_pressed) self._widget.gohome_push_button.pressed.connect(self._on_gohome_pressed) + self.circle_send_button.pressed.connect(self._on_circle_pressed) + self.lissajous_send_button.pressed.connect(self._on_lissajous_pressed) + self.stop_traj_button.pressed.connect(self._on_hover_pressed) + + def _build_trajectory_tab(self, parent_widget): + root_layout = QVBoxLayout(parent_widget) + + circle_group = QGroupBox('Circle Trajectory') + circle_layout = QGridLayout(circle_group) + + self.circle_ax_spinbox = QDoubleSpinBox() + self.circle_ax_spinbox.setRange(-50.0, 50.0) + self.circle_ax_spinbox.setSingleStep(0.1) + self.circle_ax_spinbox.setValue(1.0) + + self.circle_ay_spinbox = QDoubleSpinBox() + self.circle_ay_spinbox.setRange(-50.0, 50.0) + self.circle_ay_spinbox.setSingleStep(0.1) + self.circle_ay_spinbox.setValue(1.0) + + self.circle_period_spinbox = QDoubleSpinBox() + self.circle_period_spinbox.setRange(0.1, 300.0) + self.circle_period_spinbox.setSingleStep(0.1) + self.circle_period_spinbox.setValue(6.0) + + self.circle_duration_spinbox = QDoubleSpinBox() + self.circle_duration_spinbox.setRange(0.1, 1200.0) + self.circle_duration_spinbox.setSingleStep(0.5) + self.circle_duration_spinbox.setValue(20.0) + + self.circle_send_button = QPushButton('Start Circle') + + circle_layout.addWidget(QLabel('Ax'), 0, 0) + circle_layout.addWidget(self.circle_ax_spinbox, 0, 1) + circle_layout.addWidget(QLabel('Ay'), 0, 2) + circle_layout.addWidget(self.circle_ay_spinbox, 0, 3) + circle_layout.addWidget(QLabel('Period T [s]'), 1, 0) + circle_layout.addWidget(self.circle_period_spinbox, 1, 1) + circle_layout.addWidget(QLabel('Duration [s]'), 1, 2) + circle_layout.addWidget(self.circle_duration_spinbox, 1, 3) + circle_layout.addWidget(self.circle_send_button, 2, 0, 1, 4) + + lissajous_group = QGroupBox('Lissajous Trajectory') + lissajous_layout = QGridLayout(lissajous_group) + + self.liss_x_amp_spinbox = QDoubleSpinBox() + self.liss_x_amp_spinbox.setRange(-50.0, 50.0) + self.liss_x_amp_spinbox.setSingleStep(0.1) + self.liss_x_amp_spinbox.setValue(1.0) + + self.liss_y_amp_spinbox = QDoubleSpinBox() + self.liss_y_amp_spinbox.setRange(-50.0, 50.0) + self.liss_y_amp_spinbox.setSingleStep(0.1) + self.liss_y_amp_spinbox.setValue(0.8) + + self.liss_z_amp_spinbox = QDoubleSpinBox() + self.liss_z_amp_spinbox.setRange(-50.0, 50.0) + self.liss_z_amp_spinbox.setSingleStep(0.1) + self.liss_z_amp_spinbox.setValue(0.3) + + self.liss_yaw_amp_spinbox = QDoubleSpinBox() + self.liss_yaw_amp_spinbox.setRange(-3.14, 3.14) + self.liss_yaw_amp_spinbox.setSingleStep(0.1) + self.liss_yaw_amp_spinbox.setValue(0.0) + + self.liss_x_periods_spinbox = QDoubleSpinBox() + self.liss_x_periods_spinbox.setRange(0.1, 20.0) + self.liss_x_periods_spinbox.setSingleStep(0.1) + self.liss_x_periods_spinbox.setValue(1.0) + + self.liss_y_periods_spinbox = QDoubleSpinBox() + self.liss_y_periods_spinbox.setRange(0.1, 20.0) + self.liss_y_periods_spinbox.setSingleStep(0.1) + self.liss_y_periods_spinbox.setValue(2.0) + + self.liss_z_periods_spinbox = QDoubleSpinBox() + self.liss_z_periods_spinbox.setRange(0.1, 20.0) + self.liss_z_periods_spinbox.setSingleStep(0.1) + self.liss_z_periods_spinbox.setValue(1.0) + + self.liss_yaw_periods_spinbox = QDoubleSpinBox() + self.liss_yaw_periods_spinbox.setRange(0.0, 20.0) + self.liss_yaw_periods_spinbox.setSingleStep(0.1) + self.liss_yaw_periods_spinbox.setValue(0.0) + + self.liss_period_spinbox = QDoubleSpinBox() + self.liss_period_spinbox.setRange(0.1, 300.0) + self.liss_period_spinbox.setSingleStep(0.1) + self.liss_period_spinbox.setValue(8.0) + + self.liss_cycles_spinbox = QDoubleSpinBox() + self.liss_cycles_spinbox.setRange(0.1, 100.0) + self.liss_cycles_spinbox.setSingleStep(0.1) + self.liss_cycles_spinbox.setValue(2.0) + + self.liss_ramp_spinbox = QDoubleSpinBox() + self.liss_ramp_spinbox.setRange(0.0, 60.0) + self.liss_ramp_spinbox.setSingleStep(0.1) + self.liss_ramp_spinbox.setValue(1.0) + + self.lissajous_send_button = QPushButton('Start Lissajous') + self.stop_traj_button = QPushButton('Stop Trajectory -> Hover') + + lissajous_layout.addWidget(QLabel('x_amp'), 0, 0) + lissajous_layout.addWidget(self.liss_x_amp_spinbox, 0, 1) + lissajous_layout.addWidget(QLabel('y_amp'), 0, 2) + lissajous_layout.addWidget(self.liss_y_amp_spinbox, 0, 3) + lissajous_layout.addWidget(QLabel('z_amp'), 0, 4) + lissajous_layout.addWidget(self.liss_z_amp_spinbox, 0, 5) + + lissajous_layout.addWidget(QLabel('yaw_amp'), 1, 0) + lissajous_layout.addWidget(self.liss_yaw_amp_spinbox, 1, 1) + lissajous_layout.addWidget(QLabel('x_num_periods'), 1, 2) + lissajous_layout.addWidget(self.liss_x_periods_spinbox, 1, 3) + lissajous_layout.addWidget(QLabel('y_num_periods'), 1, 4) + lissajous_layout.addWidget(self.liss_y_periods_spinbox, 1, 5) + + lissajous_layout.addWidget(QLabel('z_num_periods'), 2, 0) + lissajous_layout.addWidget(self.liss_z_periods_spinbox, 2, 1) + lissajous_layout.addWidget(QLabel('yaw_num_periods'), 2, 2) + lissajous_layout.addWidget(self.liss_yaw_periods_spinbox, 2, 3) + lissajous_layout.addWidget(QLabel('period [s]'), 2, 4) + lissajous_layout.addWidget(self.liss_period_spinbox, 2, 5) + + lissajous_layout.addWidget(QLabel('num_cycles'), 3, 0) + lissajous_layout.addWidget(self.liss_cycles_spinbox, 3, 1) + lissajous_layout.addWidget(QLabel('ramp_time [s]'), 3, 2) + lissajous_layout.addWidget(self.liss_ramp_spinbox, 3, 3) + lissajous_layout.addWidget(self.lissajous_send_button, 4, 0, 1, 6) + lissajous_layout.addWidget(self.stop_traj_button, 5, 0, 1, 6) + + root_layout.addWidget(circle_group) + root_layout.addWidget(lissajous_group) + + def _call_service(self, service_type, service_path, request): + client = self._context.node.create_client(service_type, service_path) + if not client.wait_for_service(1.0): + self._context.node.get_logger().error(f"Service {service_path} not available") + return None + + future = client.call_async(request) + rclpy.spin_until_future_complete(self._context.node, future) + + try: + return future.result() + except Exception as e: + self._context.node.get_logger().error("Service call failed %r" % (e,)) + return None + def _on_robot_name_changed(self, robot_name): self.robot_name = str(robot_name) @@ -250,6 +424,37 @@ def _on_goto_pressed(self): except Exception as e: self._context.node.get_logger().error("Service call failed %r" % (e,)) + def _on_circle_pressed(self): + request = kr_mav_manager.srv.Circle.Request() + request.ax = self.circle_ax_spinbox.value() + request.ay = self.circle_ay_spinbox.value() + request.t = self.circle_period_spinbox.value() + request.duration = self.circle_duration_spinbox.value() + + circle_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/circle' + response = self._call_service(kr_mav_manager.srv.Circle, circle_topic, request) + if response is not None: + print('Circle: ', response.success, response.message) + + def _on_lissajous_pressed(self): + request = kr_mav_manager.srv.Lissajous.Request() + request.x_amp = self.liss_x_amp_spinbox.value() + request.y_amp = self.liss_y_amp_spinbox.value() + request.z_amp = self.liss_z_amp_spinbox.value() + request.yaw_amp = self.liss_yaw_amp_spinbox.value() + request.x_num_periods = self.liss_x_periods_spinbox.value() + request.y_num_periods = self.liss_y_periods_spinbox.value() + request.z_num_periods = self.liss_z_periods_spinbox.value() + request.yaw_num_periods = self.liss_yaw_periods_spinbox.value() + request.period = self.liss_period_spinbox.value() + request.num_cycles = self.liss_cycles_spinbox.value() + request.ramp_time = self.liss_ramp_spinbox.value() + + lissajous_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/lissajous' + response = self._call_service(kr_mav_manager.srv.Lissajous, lissajous_topic, request) + if response is not None: + print('Lissajous: ', response.success, response.message) + # Qt Methods def shutdown_plugin(self): @@ -267,6 +472,6 @@ def restore_settings(self, plugin_settings, instance_settings): self._widget.robot_name_line_edit.setText(param_value) value = instance_settings.value('node_name', "mav_services") - self.node_name = value + self.mav_node_name = value self._widget.node_name_line_edit.setText(value) diff --git a/trackers/kr_trackers/CMakeLists.txt b/trackers/kr_trackers/CMakeLists.txt index c3b731b9..94ef55c8 100644 --- a/trackers/kr_trackers/CMakeLists.txt +++ b/trackers/kr_trackers/CMakeLists.txt @@ -49,7 +49,11 @@ add_library(${PROJECT_NAME}_plugins SHARED src/null_tracker.cpp src/poly_tracker.cpp src/velocity_tracker.cpp - src/initial_conditions.cpp) + src/initial_conditions.cpp + src/circle_tracker_server.cpp + src/lissajous_generator.cpp + src/lissajous_tracker_server.cpp + src/lissajous_adder_server.cpp) ament_target_dependencies(${PROJECT_NAME}_plugins ${node_dependencies}) diff --git a/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h b/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h index 02a75ad3..7ebd0144 100644 --- a/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h +++ b/trackers/kr_trackers/include/kr_trackers/lissajous_generator.h @@ -1,39 +1,35 @@ -// TODO: convert to hpp and compatible with ros2 +#pragma once -#ifndef _LISSAJOUS_GENERATOR_H_ -#define _LISSAJOUS_GENERATOR_H_ - -#include -#include -#include -#include -#include -#include +#include "geometry_msgs/msg/point.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/lissajous_adder.hpp" +#include "kr_tracker_msgs/action/lissajous_tracker.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" class LissajousGenerator { public: - LissajousGenerator(void); - void setParams(const kr_tracker_msgs::LissajousTrackerGoal::ConstPtr &msg); - void setParams(const kr_tracker_msgs::LissajousAdderGoal::ConstPtr &msg, int num); - void generatePath(nav_msgs::Path &path, geometry_msgs::Point &initial_pt, double dt); - const kr_mav_msgs::PositionCommand::Ptr getPositionCmd(void); - bool activate(void); - void deactivate(void); - bool isActive(void); - bool goalIsSet(void); - bool status(void) const; - float timeRemaining(void); - float timeElapsed(void); + LissajousGenerator(); + void setParams(const std::shared_ptr &msg); + void setParams(const std::shared_ptr &msg, int num); + void generatePath(nav_msgs::msg::Path &path, const geometry_msgs::msg::Point &initial_pt, double dt); + kr_mav_msgs::msg::PositionCommand::SharedPtr getPositionCmd(); + bool activate(); + void deactivate(); + bool isActive() const; + bool goalIsSet() const; + bool status() const; + float timeRemaining() const; + float timeElapsed() const; private: - double lissajous_period_, ramp_time_, total_time_, ramp_s_, total_s_, const_time_, period_; + double ramp_time_, total_time_, ramp_s_, total_s_, const_time_, period_; double x_amp_, y_amp_, z_amp_, yaw_amp_; double x_num_periods_, y_num_periods_, z_num_periods_, yaw_num_periods_; double a7_, a6_, a5_, a4_; double num_cycles_; bool active_, goal_set_, goal_reached_; - ros::Time start_time_; + rclcpp::Time start_time_; + rclcpp::Clock::SharedPtr clock_; }; - -#endif diff --git a/trackers/kr_trackers/plugins_description.xml b/trackers/kr_trackers/plugins_description.xml index b3106477..d165a8b9 100644 --- a/trackers/kr_trackers/plugins_description.xml +++ b/trackers/kr_trackers/plugins_description.xml @@ -25,6 +25,21 @@ base_class_type="kr_trackers_manager::Tracker"> This is a tracker which follows a velocity command. + + + This is a tracker which follows an elliptical trajectory defined by amplitudes and period. + + + + This is a tracker which follows a Lissajous trajectory. + + + + This is a tracker which follows a sum of two Lissajous trajectories. + diff --git a/trackers/kr_trackers/src/circle_tracker_server.cpp b/trackers/kr_trackers/src/circle_tracker_server.cpp index 4b784de0..500402bb 100644 --- a/trackers/kr_trackers/src/circle_tracker_server.cpp +++ b/trackers/kr_trackers/src/circle_tracker_server.cpp @@ -1,189 +1,155 @@ -/* Sarah Tang - * Trajectory tracker to track an ellipse of given height/width and period. - * x(t) = [Ax*cos(2*pi*t/T); Ay*sin(2*pi*t/T); 0] + init_pos. - * Yaw is constant. */ - -// TODO: convert to ros2 compatible format - -#include -#include -#include -#include -#include -#include -#include -#include +/* + * Trajectory tracker for elliptical/circular motion: + * x(t) = [Ax*cos(2*pi*t/T), Ay*sin(2*pi*t/T), 0] + offset. + */ + +#include "kr_trackers/Tracker.hpp" + +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/circle_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "std_msgs/msg/empty.hpp" +#include "tf2/utils.h" #include #include +#include +#include class CircleTracker : public kr_trackers_manager::Tracker { public: - CircleTracker(void); + CircleTracker() = default; - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void Deactivate(); + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) override; + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) override; + void Deactivate() override; - kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); - - uint8_t status() const; + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg) override; + uint8_t status() override; private: - void goal_callback(); - void preempt_callback(); - - typedef actionlib::SimpleActionServer ServerType; - // Action server that takes a trajectory. - // Must be a pointer, because plugin does not support a constructor - // with inputs, but an action server must be initialized with a Nodehandle. - std::shared_ptr tracker_server_; - - // Is the tracker active? - bool active_; - // Do we have odometry? - bool have_odom_; - - // Time trajectory started. - ros::Time traj_start_time_; - // traj_started_ indicates whether to reset start_time_. - bool traj_started_; - - // Only track for position. - // Distance traveled to get to last goal. - float current_traj_length_; - - // Coefficients are ordered such that omega(t) = traj_coeffs_[i]*t^i. - std::vector omega_coeffs_; - float Ax_, Ay_, omega_des_; - - // traj_completed_ indicates whether the trajectory is completed. - bool traj_completed_; - // Total time we want to track the trajectory for. - float traj_duration_; - // Time it takes to ramp up to the desired angular velocity. - // ramp_dist_ is the theta at the end of the ramp phase. - float ramp_time_, ramp_dist_; - // Time that robot should stop circling (includes ramp up time). - // circle_dist_ is the theta at the end of both phases. - float circle_time_, circle_dist_; - // Average angular acceleration for ramp up. - float alpha_des_; - - // This is set if traj_completed_ = true; - // Save the final position so we don't have to keep evaluating. - // This holds the final position of the trajectory itself: - // if use_offset_ = true, then the robot will evaluate position to - // pos = final_pos_ + offset_pos_. - Eigen::Vector3f final_pos_; - - // Offset to apply to the trajectory position. - Eigen::Vector3f offset_pos_; - - // Record the current state. - Eigen::Vector3f current_pos_; - float current_yaw_; - float constant_yaw_; - - // Pubish trigger for start and goal. - ros::Publisher pub_start_; - ros::Publisher pub_end_; + using CircleTrackerAction = kr_tracker_msgs::action::CircleTracker; + using CircleTrackerGoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + void handle_accepted_callback(const std::shared_ptr goal_handle); + + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + rclcpp::Clock::SharedPtr clock_; + + rclcpp_action::Server::SharedPtr tracker_server_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + std::shared_ptr current_goal_handle_; + std::recursive_mutex mutex_; + + rclcpp::Publisher::SharedPtr pub_start_; + rclcpp::Publisher::SharedPtr pub_end_; + + bool active_{false}; + bool have_odom_{false}; + bool traj_started_{false}; + bool traj_completed_{false}; + + rclcpp::Time traj_start_time_; + + float current_traj_length_{0.0f}; + float ax_{0.0f}; + float ay_{0.0f}; + float period_{1.0f}; + float traj_duration_{0.0f}; + float omega_{0.0f}; + float ramp_up_time_{2.0f}; + + Eigen::Vector3f offset_pos_{Eigen::Vector3f::Zero()}; + Eigen::Vector3f final_pos_{Eigen::Vector3f::Zero()}; + Eigen::Vector3f current_pos_{Eigen::Vector3f::Zero()}; + float current_yaw_{0.0f}; + float constant_yaw_{0.0f}; }; -CircleTracker::CircleTracker(void) - : active_(false), - have_odom_(false), - traj_started_(false), - current_traj_length_(0.0), - Ax_(0.0), - Ay_(0.0), - omega_des_(0.0), - traj_completed_(false), - traj_duration_(-1.0), - ramp_time_(-1.0), - circle_time_(-1.0) +void CircleTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + node->declare_parameter("circle_tracker/ramp_up_time", 2.0); + ramp_up_time_ = static_cast(node->get_parameter("circle_tracker/ramp_up_time").as_double()); + + pub_start_ = node->create_publisher("~/circle_tracker/traj_start", 10); + pub_end_ = node->create_publisher("~/circle_tracker/traj_end", 10); + + cb_group_ = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + tracker_server_ = rclcpp_action::create_server( + node, + "~/circle_tracker/CircleTracker", + std::bind(&CircleTracker::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CircleTracker::cancel_callback, this, std::placeholders::_1), + std::bind(&CircleTracker::handle_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); + + RCLCPP_INFO(logger_, "Initialized CircleTracker"); } -void CircleTracker::Initialize(const ros::NodeHandle &nh) +bool CircleTracker::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { - ros::NodeHandle priv_nh(nh, "circle_tracker"); - - priv_nh.param("alpha_des", alpha_des_, static_cast(M_PI / 20.0)); - - // Set up the action server. - tracker_server_ = std::shared_ptr(new ServerType(priv_nh, "CircleTracker", false)); - tracker_server_->registerGoalCallback(boost::bind(&CircleTracker::goal_callback, this)); - tracker_server_->registerPreemptCallback(boost::bind(&CircleTracker::preempt_callback, this)); + (void)cmd; + std::lock_guard lock(mutex_); - tracker_server_->start(); - - pub_start_ = priv_nh.advertise("traj_start", 10); - pub_end_ = priv_nh.advertise("traj_end", 10); -} - -bool CircleTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) -{ if(!have_odom_) { - ROS_WARN("CircleTracker::Activate: could not activate because no odom recieved - not activating."); + RCLCPP_WARN(logger_, "CircleTracker::Activate failed: no odometry yet."); active_ = false; - return active_; + return false; } - if(!tracker_server_->isActive()) + if(!current_goal_handle_ || !current_goal_handle_->is_active()) { - ROS_WARN("CircleTracker::Activate: server has no active goal - not activating."); + RCLCPP_WARN(logger_, "CircleTracker::Activate failed: no active goal."); active_ = false; - return active_; - } - - if(omega_coeffs_.empty()) - { - ROS_WARN("CircleTracker::Activate: internal trajectory not initialized - not activating."); - active_ = false; - return active_; + return false; } active_ = true; traj_started_ = false; - current_traj_length_ = 0.0; traj_completed_ = false; + current_traj_length_ = 0.0f; - // Calculate the offset needed so that - // x(0) + offset = current_pos_. - // Here, x(0) is [Ax 0 0]. - offset_pos_ = current_pos_ - Eigen::Vector3f(Ax_, 0.0, 0.0); + // Center trajectory on activation pose. + offset_pos_ = current_pos_; constant_yaw_ = current_yaw_; - // std::cout << " RELATIVE ACTIVATE CALCULATED AS " << offset_pos_(0) << " " << offset_pos_(1) << " " << - // offset_pos_(2) << " " << offset_yaw_ << "\n"; - - return active_; + return true; } -void CircleTracker::Deactivate(void) +void CircleTracker::Deactivate() { - if(tracker_server_->isActive()) - { - ROS_WARN("CircleTracker::Deactivate: deactivated tracker while still tracking position trajectory."); + std::lock_guard lock(mutex_); - kr_tracker_msgs::CircleTrackerResult result; - result.duration = std::max(0.0f, static_cast((ros::Time::now() - traj_start_time_).toSec())); - result.length = current_traj_length_; - tracker_server_->setAborted(result); + if(current_goal_handle_ && current_goal_handle_->is_active()) + { + auto result = std::make_shared(); + result->duration = std::max(0.0f, static_cast((clock_->now() - traj_start_time_).seconds())); + result->length = current_traj_length_; + current_goal_handle_->abort(result); + current_goal_handle_.reset(); } active_ = false; - have_odom_ = false; traj_started_ = false; - current_traj_length_ = 0.0; traj_completed_ = false; + current_traj_length_ = 0.0f; } -kr_mav_msgs::PositionCommand::ConstPtr CircleTracker::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr CircleTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { - // Record distance between last position and current. + std::lock_guard lock(mutex_); + const float dx = Eigen::Vector3f((current_pos_(0) - msg->pose.pose.position.x), (current_pos_(1) - msg->pose.pose.position.y), (current_pos_(2) - msg->pose.pose.position.z)) @@ -192,337 +158,212 @@ kr_mav_msgs::PositionCommand::ConstPtr CircleTracker::update(const nav_msgs::Odo current_pos_(0) = msg->pose.pose.position.x; current_pos_(1) = msg->pose.pose.position.y; current_pos_(2) = msg->pose.pose.position.z; - current_yaw_ = tf::getYaw(msg->pose.pose.orientation); - + current_yaw_ = tf2::getYaw(msg->pose.pose.orientation); have_odom_ = true; if(!active_) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } current_traj_length_ += dx; - const ros::Time t_now = ros::Time::now(); - kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand); - cmd->header.stamp = t_now; - cmd->header.frame_id = msg->header.frame_id; - if(!traj_started_) { - // Start the trajectory. - traj_start_time_ = ros::Time::now(); + traj_start_time_ = clock_->now(); traj_started_ = true; traj_completed_ = false; - - current_traj_length_ = 0.0; + current_traj_length_ = 0.0f; } - const float traj_time = std::max(0.0f, static_cast((ros::Time::now() - traj_start_time_).toSec())); - constexpr float kEps = 1e-6; + const float t = std::max(0.0f, static_cast((clock_->now() - traj_start_time_).seconds())); + const float t_eval = std::min(t, traj_duration_); + + const float ramp_t = std::max(0.0f, std::min(ramp_up_time_, traj_duration_)); + float s = 1.0f; + float s_dot = 0.0f; + float s_ddot = 0.0f; + float s_dddot = 0.0f; - // Process position trajectory. - if(traj_completed_) + if(ramp_t > 1e-4f && t_eval < ramp_t) { - cmd->position.x = final_pos_(0); - cmd->position.y = final_pos_(1); - cmd->position.z = final_pos_(2); - - cmd->yaw = constant_yaw_; - - cmd->velocity.x = 0.0; - cmd->velocity.y = 0.0; - cmd->velocity.z = 0.0; - cmd->acceleration.x = 0.0; - cmd->acceleration.y = 0.0; - cmd->acceleration.z = 0.0; - cmd->jerk.x = 0.0; - cmd->jerk.y = 0.0; - cmd->jerk.z = 0.0; - - cmd->yaw_dot = 0.0; + const float tau = std::max(0.0f, std::min(1.0f, t_eval / ramp_t)); + const float tau2 = tau * tau; + const float tau3 = tau2 * tau; + const float tau4 = tau3 * tau; + + // smooth ramp up + s = 10.0f * tau3 - 15.0f * tau4 + 6.0f * tau4 * tau; + s_dot = (30.0f * tau2 - 60.0f * tau3 + 30.0f * tau4) / ramp_t; + s_ddot = (60.0f * tau - 180.0f * tau2 + 120.0f * tau3) / (ramp_t * ramp_t); + s_dddot = (60.0f - 360.0f * tau + 360.0f * tau2) / (ramp_t * ramp_t * ramp_t); } - else - { - float theta, theta_dot, theta_ddot, theta_dddot; - - // theta = omega_des_ * traj_time; - // theta_dot = omega_des_; - // theta_ddot = 0.0; - // theta_dddot = 0.0; - // theta_d4dot = 0.0; - // theta_d5dot = 0.0; - // theta_d6dot = 0.0; - - // Ramping up the angular velocity. - if(traj_time < ramp_time_) - { - // ROS_INFO("Circle Tracker: Accelerating..."); - - /* theta = 0.5 * alpha_des_ * traj_time_ * traj_time_; - theta_dot = alpha_des_ * traj_time_; - theta_ddot = alpha_des_; - theta_dddot = 0.0;*/ - const float dT = traj_time / ramp_time_; - // std::cout << " ramp time i s" << ramp_time_ << " and dt is " << dT << "\n"; - const float dT2 = dT * dT, dT3 = dT2 * dT, dT4 = dT3 * dT, dT5 = dT4 * dT, dT6 = dT5 * dT; - theta = omega_coeffs_.at(0) * dT + 0.5 * omega_coeffs_.at(1) * dT2 + 1.0 / 3.0 * omega_coeffs_.at(2) * dT3 + - 0.25 * omega_coeffs_.at(3) * dT4 + 1.0 / 5.0 * omega_coeffs_.at(4) * dT5 + - 1.0 / 6.0 * omega_coeffs_.at(5) * dT6; - theta_dot = omega_coeffs_.at(0) + omega_coeffs_.at(1) * dT + omega_coeffs_.at(2) * dT2 + - omega_coeffs_.at(3) * dT3 + omega_coeffs_.at(4) * dT4 + omega_coeffs_.at(5) * dT5; - theta_dot /= ramp_time_; - theta_ddot = omega_coeffs_.at(1) + 2.0 * omega_coeffs_.at(2) * dT + 3.0 * omega_coeffs_.at(3) * dT2 + - 4.0 * omega_coeffs_.at(4) * dT3 + 5.0 * omega_coeffs_.at(5) * dT4; - const float ramp_time_2 = ramp_time_ * ramp_time_; - theta_ddot /= ramp_time_2; - theta_dddot = 2.0 * omega_coeffs_.at(2) + 6.0 * omega_coeffs_.at(3) * dT + 12.0 * omega_coeffs_.at(4) * dT2 + - 20.0 * omega_coeffs_.at(5) * dT3; - const float ramp_time_3 = ramp_time_2 * ramp_time_; - theta_dddot /= (ramp_time_3); - } - // Constant segment. - else if(traj_time < circle_time_) - { - // ROS_INFO("Circle Tracker: Coasting..."); - std_msgs::Empty empty_msg; - pub_start_.publish(empty_msg); - // Calculate time in this phase. - const float dT = traj_time - ramp_time_; + const float theta = omega_ * t_eval; + const float sin_t = std::sin(theta); + const float cos_t = std::cos(theta); - theta = omega_des_ * dT + ramp_dist_; - theta_dot = omega_des_; - theta_ddot = 0.0; - theta_dddot = 0.0; - } - // Ramping down the angular velocity. - else - { - // const float dT = traj_time - circle_time_; - // theta = omega_des_ * dT + circle_dist_; - // theta_dot = omega_des_; - // theta_ddot = 0.0; - // theta_dddot = 0.0; - // theta_d4dot = 0.0; - // theta_d5dot = 0.0; - // theta_d6dot = 0.0; - - // ROS_INFO("Circle Tracker: Decelerating..."); - - // Publish message indicating trajectory end. - std_msgs::Empty empty_msg; - pub_end_.publish(empty_msg); - - // const float dT = traj_time - circle_time_; - // theta = -0.5 * alpha_des_ * dT * dT + omega_des_ * dT + circle_dist_; - // theta_dot = -alpha_des_ * dT + omega_des_; - // theta_ddot = -alpha_des_; - // theta_dddot = 0.0; - - const float dT = 1.0 - (traj_time - circle_time_) / ramp_time_; - const float dT2 = dT * dT, dT3 = dT2 * dT, dT4 = dT3 * dT, dT5 = dT4 * dT, dT6 = dT5 * dT; - theta = omega_coeffs_.at(0) * dT + 0.5 * omega_coeffs_.at(1) * dT2 + 1.0 / 3.0 * omega_coeffs_.at(2) * dT3 + - 0.25 * omega_coeffs_.at(3) * dT4 + 1.0 / 5.0 * omega_coeffs_.at(4) * dT5 + - 1.0 / 6.0 * omega_coeffs_.at(5) * dT6; - theta = circle_dist_ + ramp_dist_ - theta; - theta_dot = omega_coeffs_.at(0) + omega_coeffs_.at(1) * dT + omega_coeffs_.at(2) * dT2 + - omega_coeffs_.at(3) * dT3 + omega_coeffs_.at(4) * dT4 + omega_coeffs_.at(5) * dT5; - theta_dot /= ramp_time_; - theta_ddot = omega_coeffs_.at(1) + 2.0 * omega_coeffs_.at(2) * dT + 3.0 * omega_coeffs_.at(3) * dT2 + - 4.0 * omega_coeffs_.at(4) * dT3 + 5.0 * omega_coeffs_.at(5) * dT4; - const float ramp_time_2 = ramp_time_ * ramp_time_; - theta_ddot /= ramp_time_2; - theta_dddot = 2.0 * omega_coeffs_.at(2) + 6.0 * omega_coeffs_.at(3) * dT + 12.0 * omega_coeffs_.at(4) * dT2 + - 20.0 * omega_coeffs_.at(5) * dT3; - const float ramp_time_3 = ramp_time_2 * ramp_time_; - theta_dddot /= (ramp_time_3); - } + float pos_x = ax_ * s * cos_t; + float pos_y = ay_ * s * sin_t; + float pos_z = 0.0f; - const float sin_t = sin(theta); - const float cos_t = cos(theta); + float vel_x = ax_ * (s_dot * cos_t - s * sin_t * omega_); + float vel_y = ay_ * (s_dot * sin_t + s * cos_t * omega_); + float vel_z = 0.0f; - const float pos_x = Ax_ * cos_t; - const float pos_y = Ay_ * sin_t; - const float pos_z = 0.0; + const float omega2 = omega_ * omega_; + float acc_x = ax_ * (s_ddot * cos_t - 2.0f * s_dot * sin_t * omega_ - s * cos_t * omega2); + float acc_y = ay_ * (s_ddot * sin_t + 2.0f * s_dot * cos_t * omega_ - s * sin_t * omega2); + float acc_z = 0.0f; - const float vel_x = Ax_ * (-sin_t * theta_dot); - const float vel_y = Ay_ * (cos_t * theta_dot); - const float vel_z = 0.0; + const float omega3 = omega2 * omega_; + float jerk_x = + ax_ * (s_dddot * cos_t - 3.0f * s_ddot * sin_t * omega_ - 3.0f * s_dot * cos_t * omega2 + s * sin_t * omega3); + float jerk_y = + ay_ * (s_dddot * sin_t + 3.0f * s_ddot * cos_t * omega_ - 3.0f * s_dot * sin_t * omega2 - s * cos_t * omega3); + float jerk_z = 0.0f; - const float theta_dot_2 = theta_dot * theta_dot; - const float acc_x = Ax_ * (-cos_t * theta_dot_2 - sin_t * theta_ddot); - const float acc_y = Ay_ * (-sin_t * theta_dot_2 + cos_t * theta_ddot); - const float acc_z = 0.0; + if(t >= traj_duration_) + { + traj_completed_ = true; + final_pos_ = Eigen::Vector3f(pos_x, pos_y, pos_z); + + pos_x = final_pos_(0); + pos_y = final_pos_(1); + pos_z = final_pos_(2); + vel_x = 0.0f; + vel_y = 0.0f; + vel_z = 0.0f; + acc_x = 0.0f; + acc_y = 0.0f; + acc_z = 0.0f; + jerk_x = 0.0f; + jerk_y = 0.0f; + jerk_z = 0.0f; + + std_msgs::msg::Empty end_msg; + pub_end_->publish(end_msg); + } + else + { + std_msgs::msg::Empty start_msg; + pub_start_->publish(start_msg); + } - const float theta_dot_3 = theta_dot_2 * theta_dot; - const float jerk_x = Ax_ * (sin_t * theta_dot_3 - 3.0 * cos_t * theta_dot * theta_ddot - sin_t * theta_dddot); - const float jerk_y = Ay_ * (-cos_t * theta_dot_3 - 3.0 * sin_t * theta_dot * theta_ddot + cos_t * theta_dddot); - const float jerk_z = 0.0; + auto cmd = std::make_shared(); + cmd->header.stamp = clock_->now(); + cmd->header.frame_id = msg->header.frame_id; - // Note, these values do NOT yet include the offset. - cmd->position.x = pos_x; - cmd->position.y = pos_y; - cmd->position.z = pos_z; - cmd->yaw = constant_yaw_; + cmd->position.x = pos_x + offset_pos_(0); + cmd->position.y = pos_y + offset_pos_(1); + cmd->position.z = pos_z + offset_pos_(2); + cmd->yaw = constant_yaw_; - cmd->velocity.x = vel_x; - cmd->velocity.y = vel_y; - cmd->velocity.z = vel_z; - cmd->yaw_dot = 0.0; + cmd->velocity.x = vel_x; + cmd->velocity.y = vel_y; + cmd->velocity.z = vel_z; + cmd->yaw_dot = 0.0f; - cmd->acceleration.x = acc_x; - cmd->acceleration.y = acc_y; - cmd->acceleration.z = acc_z; + cmd->acceleration.x = acc_x; + cmd->acceleration.y = acc_y; + cmd->acceleration.z = acc_z; - cmd->jerk.x = jerk_x; - cmd->jerk.y = jerk_y; - cmd->jerk.z = jerk_z; + cmd->jerk.x = jerk_x; + cmd->jerk.y = jerk_y; + cmd->jerk.z = jerk_z; - // Check if we completed the trajectory. - if(traj_time >= traj_duration_ - kEps) + if(current_goal_handle_ && current_goal_handle_->is_active()) + { + if(traj_completed_) { - traj_completed_ = true; - final_pos_ = Eigen::Vector3f(pos_x, pos_y, pos_z); + auto result = std::make_shared(); + result->duration = t; + result->length = current_traj_length_; + current_goal_handle_->succeed(result); + + active_ = false; + current_goal_handle_.reset(); + current_traj_length_ = 0.0f; + } + else + { + auto feedback = std::make_shared(); + feedback->duration = t; + current_goal_handle_->publish_feedback(feedback); } - } - - cmd->position.x += offset_pos_(0); - cmd->position.y += offset_pos_(1); - cmd->position.z += offset_pos_(2); - - // If trajectory is completed, indicate this in the action. - if(tracker_server_->isActive() && traj_completed_) - { - kr_tracker_msgs::CircleTrackerResult result; - result.duration = traj_time; - result.length = current_traj_length_; - tracker_server_->setSucceeded(result); - current_traj_length_ = 0.0; - } - // Otherwise, send feedback command. - else - { - kr_tracker_msgs::CircleTrackerFeedback feedback; - feedback.duration = traj_time; - tracker_server_->publishFeedback(feedback); } return cmd; } -void CircleTracker::goal_callback() +uint8_t CircleTracker::status() { - // If another goal is already active, cancel that goal - // and track this one instead. - if(tracker_server_->isActive()) + std::lock_guard lock(mutex_); + if(active_ && current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_INFO("CircleTracker trajectory aborted because new goal recieved."); - kr_tracker_msgs::CircleTrackerResult result; - result.duration = std::max(0.0f, static_cast((ros::Time::now() - traj_start_time_).toSec())); - result.length = current_traj_length_; - - tracker_server_->setAborted(result); + return static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE); } + return static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); +} - // Pointer to the goal recieved. - const auto msg = tracker_server_->acceptNewGoal(); - - // If preempt has been requested, then set this goal to preempted - // and make no changes to the tracker state. - if(tracker_server_->isPreemptRequested()) - { - ROS_INFO("CircleTracker trajectory preempted immediately after it was recieved."); - kr_tracker_msgs::CircleTrackerResult result; - result.duration = 0.0; - result.length = 0.0; - tracker_server_->setPreempted(result); - return; - } +rclcpp_action::GoalResponse CircleTracker::goal_callback( + const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) +{ + (void)uuid; - // Calculate the trajectory. - omega_des_ = 2.0 * M_PI / msg->T; - Ax_ = msg->Ax; - Ay_ = msg->Ay; - // Calculate the ramp time. - const float dT = omega_des_ / alpha_des_; - - Eigen::MatrixXf Ainv(6, 6); - Ainv << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, -10.0, -6.0, -1.5, - 10.0, -4.0, 0.5, 15.0, 8.0, 1.5, -15.0, 7.0, -1.0, -6.0, -3.0, -0.5, 6.0, -3.0, 0.5; - Eigen::VectorXf b(6); - b << 0.0, 0.0, 0.0, omega_des_ * dT, 0.0, 0.0; - // std::cout << "A_inv" << Ainv(3, 0) << " " << Ainv(3, 1) << " " << Ainv(3, 2) << " " << Ainv(3, 3) << " " << Ainv(3, - // 4) << " " << Ainv(3, 5) << " and b " << b(3) << "\n"; - Eigen::VectorXf omega_coeffs_vec = Ainv * b; - // std::cout << " rowwise multiply " << Ainv.row(3) * b << "\n"; - omega_coeffs_.resize(6); - // Track the angle traveled when ramping up. - // We have coefficients for omega(t). - // We need to evaluate theta(t) = int_0^t omega(t) at 1. - ramp_dist_ = 0.0; - // Scale the trajectory back. - // const std::vector T_powers = {1, dT, dT * dT, dT*dT * dT, dT*dT*dT * dT, dT*dT*dT*dT * dT}; - for(int ii = 0; ii < 6; ++ii) + if(goal->t <= 0.0 || goal->duration <= 0.0) { - // std::cout << " the condition here is " << b(ii) << "\n"; - omega_coeffs_.at(ii) = omega_coeffs_vec(ii); // omega_coeffs_vec(ii) / T_powers.at(ii); - // std::cout << " coefficient " << ii << " is " << omega_coeffs_vec(ii) << "\n"; - ramp_dist_ += omega_coeffs_.at(ii) * 1.0 / static_cast(ii + 1); + RCLCPP_WARN(logger_, "Rejecting CircleTracker goal with non-positive period/duration"); + return rclcpp_action::GoalResponse::REJECT; } - // Calculate the offset needed so that - // x(0) + offset = current_pos_. - offset_pos_ = current_pos_ - Eigen::Vector3f(Ax_, 0.0, 0.0); - // std::cout << " RELATIVE CALCULATED AS " << offset_pos_(0) << " " << offset_pos_(1) << " " << offset_pos_(2) << " " - // << offset_yaw_ << "\n"; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} - ramp_time_ = dT; - circle_time_ = msg->duration + ramp_time_; - traj_duration_ = 2.0 * ramp_time_ + msg->duration; +rclcpp_action::CancelResponse CircleTracker::cancel_callback(const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); - // Calculate the distances needed. - circle_dist_ = ramp_dist_ + omega_des_ * msg->duration; + if(current_goal_handle_ == goal_handle) + { + auto result = std::make_shared(); + result->duration = std::max(0.0f, static_cast((clock_->now() - traj_start_time_).seconds())); + result->length = current_traj_length_; + goal_handle->canceled(result); - // std::cout << " CIRCLE DESIGNED: ramp time " << ramp_time_ << " and circle time " << circle_time_ << " and total - // duration " << traj_duration_ << " the ramp dist " << ramp_dist_ * 180.0 / M_PI << " and circle dist " << - // circle_dist_ * 180.0 / M_PI << " to get to angular velocity " << omega_des_ << "\n"; + active_ = false; + traj_started_ = false; + traj_completed_ = true; + current_goal_handle_.reset(); + return rclcpp_action::CancelResponse::ACCEPT; + } - traj_started_ = false; - traj_completed_ = false; + return rclcpp_action::CancelResponse::REJECT; } -void CircleTracker::preempt_callback() +void CircleTracker::handle_accepted_callback(const std::shared_ptr goal_handle) { - // Send a message reporting about the trajectory that was executed. - kr_tracker_msgs::CircleTrackerResult result; - result.duration = std::max(0.0f, static_cast((ros::Time::now() - traj_start_time_).toSec())); - result.length = current_traj_length_; + std::lock_guard lock(mutex_); - if(tracker_server_->isActive()) + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_INFO("CircleTracker trajectory aborted by preempt command."); - tracker_server_->setAborted(result); - } - else - { - ROS_INFO("CircleTracker trajectory preempted by preempt command."); - tracker_server_->setPreempted(result); + auto result = std::make_shared(); + result->duration = std::max(0.0f, static_cast((clock_->now() - traj_start_time_).seconds())); + result->length = current_traj_length_; + current_goal_handle_->abort(result); } - traj_started_ = true; - traj_completed_ = true; - traj_duration_ = 0.0; - current_traj_length_ = 0.0; - - final_pos_ = current_pos_; -} + const auto goal = goal_handle->get_goal(); + ax_ = static_cast(goal->ax); + ay_ = static_cast(goal->ay); + period_ = static_cast(goal->t); + traj_duration_ = static_cast(goal->duration); + omega_ = static_cast(2.0 * M_PI / period_); -uint8_t CircleTracker::status() const -{ - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + current_goal_handle_ = goal_handle; + traj_started_ = false; + traj_completed_ = false; + active_ = false; } -#include PLUGINLIB_EXPORT_CLASS(CircleTracker, kr_trackers_manager::Tracker); diff --git a/trackers/kr_trackers/src/lissajous_adder_server.cpp b/trackers/kr_trackers/src/lissajous_adder_server.cpp index 45546008..d1ec0fd1 100644 --- a/trackers/kr_trackers/src/lissajous_adder_server.cpp +++ b/trackers/kr_trackers/src/lissajous_adder_server.cpp @@ -1,91 +1,125 @@ -// TODO: convert to ros2 compatible format +#include "kr_trackers/Tracker.hpp" +#include "kr_trackers/initial_conditions.hpp" +#include "kr_trackers/lissajous_generator.h" -#include -#include -#include -#include -#include -#include -#include +#include "geometry_msgs/msg/point.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/lissajous_adder.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "nav_msgs/msg/path.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include -#include +#include #include +#include class LissajousAdder : public kr_trackers_manager::Tracker { public: - LissajousAdder(void); - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void Deactivate(void); + LissajousAdder() = default; - kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); - uint8_t status() const; + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) override; + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) override; + void Deactivate() override; + + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg) override; + uint8_t status() override; private: - void goal_callback(void); - void preempt_callback(void); + using LissajousAdderAction = kr_tracker_msgs::action::LissajousAdder; + using LissajousAdderGoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + void handle_accepted_callback(const std::shared_ptr goal_handle); - typedef actionlib::SimpleActionServer ServerType; - std::shared_ptr tracker_server_; - ros::Publisher path_pub_; + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp_action::Server::SharedPtr tracker_server_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + std::shared_ptr current_goal_handle_; + std::recursive_mutex mutex_; InitialConditions ICs_; - LissajousGenerator generator_1_, generator_2_; - double distance_traveled_; - Eigen::Vector3d position_last_; - bool traj_start_set_; - std::string frame_id_; + LissajousGenerator generator_1_; + LissajousGenerator generator_2_; + double distance_traveled_{0.0}; + Eigen::Vector3d position_last_{Eigen::Vector3d::Zero()}; + bool traj_start_set_{false}; + bool active_{false}; + std::string frame_id_{"odom"}; }; -LissajousAdder::LissajousAdder(void) : traj_start_set_(false) {} - -void LissajousAdder::Initialize(const ros::NodeHandle &nh) +void LissajousAdder::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { - ros::NodeHandle priv_nh(nh, "lissajous_adder"); - priv_nh.param("frame_id", frame_id_, "world"); - path_pub_ = priv_nh.advertise("lissajous_path", 1); - - tracker_server_ = std::shared_ptr(new ServerType(priv_nh, "LissajousAdder", false)); - tracker_server_->registerGoalCallback(boost::bind(&LissajousAdder::goal_callback, this)); - tracker_server_->registerPreemptCallback(boost::bind(&LissajousAdder::preempt_callback, this)); - tracker_server_->start(); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + node->declare_parameter("lissajous_adder/frame_id", "odom"); + frame_id_ = node->get_parameter("lissajous_adder/frame_id").as_string(); + + path_pub_ = node->create_publisher("~/lissajous_adder/lissajous_path", 1); + + cb_group_ = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + tracker_server_ = rclcpp_action::create_server( + node, + "~/lissajous_adder/LissajousAdder", + std::bind(&LissajousAdder::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LissajousAdder::cancel_callback, this, std::placeholders::_1), + std::bind(&LissajousAdder::handle_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); + + RCLCPP_INFO(logger_, "Initialized LissajousAdder"); } -bool LissajousAdder::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +bool LissajousAdder::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { - // Only allow activation if a goal has been set - if(generator_1_.goalIsSet() && generator_2_.goalIsSet()) + (void)cmd; + std::lock_guard lock(mutex_); + + if(generator_1_.goalIsSet() && generator_2_.goalIsSet() && current_goal_handle_ && current_goal_handle_->is_active()) { - if(!tracker_server_->isActive()) - { - ROS_WARN("LissajousAdder::Activate: goal_set is true but action server has no active goal - not activating."); - return false; - } - return (generator_1_.activate() && generator_2_.activate()); + active_ = generator_1_.activate() && generator_2_.activate(); + return active_; } + + active_ = false; return false; } -void LissajousAdder::Deactivate(void) +void LissajousAdder::Deactivate() { - if(tracker_server_->isActive()) + std::lock_guard lock(mutex_); + + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_WARN("Deactivated tracker prior to reaching goal"); - tracker_server_->setAborted(); + auto result = std::make_shared(); + result->duration = std::max(generator_1_.timeElapsed(), generator_2_.timeElapsed()); + result->length = distance_traveled_; + current_goal_handle_->abort(result); + current_goal_handle_.reset(); } + ICs_.reset(); generator_1_.deactivate(); generator_2_.deactivate(); traj_start_set_ = false; + active_ = false; } -kr_mav_msgs::PositionCommand::ConstPtr LissajousAdder::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LissajousAdder::update(const nav_msgs::msg::Odometry::SharedPtr msg) { - if(!(generator_1_.isActive() & generator_2_.isActive())) + std::lock_guard lock(mutex_); + + if(!active_ || !generator_1_.isActive() || !generator_2_.isActive()) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } if(!traj_start_set_) @@ -94,122 +128,149 @@ kr_mav_msgs::PositionCommand::ConstPtr LissajousAdder::update(const nav_msgs::Od ICs_.set_from_odom(msg); position_last_ = Eigen::Vector3d(ICs_.pos()(0), ICs_.pos()(1), ICs_.pos()(2)); - // Generate path for visualizing - geometry_msgs::Point initial_pt, initial_pt2; + geometry_msgs::msg::Point initial_pt; initial_pt.x = ICs_.pos()(0); initial_pt.y = ICs_.pos()(1); initial_pt.z = ICs_.pos()(2); - double dt = 0.1; - nav_msgs::Path path1, path2; + + nav_msgs::msg::Path path1; + nav_msgs::msg::Path path2; path1.header.frame_id = frame_id_; - path1.header.stamp = ros::Time::now(); - generator_1_.generatePath(path1, initial_pt, dt); - generator_2_.generatePath(path2, initial_pt2, dt); - for(unsigned int i = 0; i < path1.poses.size(); i++) + path1.header.stamp = clock_->now(); + + generator_1_.generatePath(path1, initial_pt, 0.1); + generator_2_.generatePath(path2, initial_pt, 0.1); + + const size_t count = std::min(path1.poses.size(), path2.poses.size()); + for(size_t i = 0; i < count; ++i) { - path1.poses[i].pose.position.x += path2.poses[i].pose.position.x; - path1.poses[i].pose.position.y += path2.poses[i].pose.position.y; - path1.poses[i].pose.position.z += path2.poses[i].pose.position.z; + path1.poses[i].pose.position.x += path2.poses[i].pose.position.x - initial_pt.x; + path1.poses[i].pose.position.y += path2.poses[i].pose.position.y - initial_pt.y; + path1.poses[i].pose.position.z += path2.poses[i].pose.position.z - initial_pt.z; } - path_pub_.publish(path1); + + path_pub_->publish(path1); } - // Set gains - kr_mav_msgs::PositionCommand::Ptr cmd1 = generator_1_.getPositionCmd(); - kr_mav_msgs::PositionCommand::Ptr cmd2 = generator_2_.getPositionCmd(); - if(cmd1 == NULL && cmd2 == NULL) + auto cmd1 = generator_1_.getPositionCmd(); + auto cmd2 = generator_2_.getPositionCmd(); + if(!cmd1 || !cmd2) { - return cmd1; + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } - else + + cmd1->header.stamp = clock_->now(); + cmd1->header.frame_id = msg->header.frame_id; + cmd1->position.x += ICs_.pos()(0) + cmd2->position.x; + cmd1->position.y += ICs_.pos()(1) + cmd2->position.y; + cmd1->position.z += ICs_.pos()(2) + cmd2->position.z; + cmd1->velocity.x += cmd2->velocity.x; + cmd1->velocity.y += cmd2->velocity.y; + cmd1->velocity.z += cmd2->velocity.z; + cmd1->acceleration.x += cmd2->acceleration.x; + cmd1->acceleration.y += cmd2->acceleration.y; + cmd1->acceleration.z += cmd2->acceleration.z; + cmd1->jerk.x += cmd2->jerk.x; + cmd1->jerk.y += cmd2->jerk.y; + cmd1->jerk.z += cmd2->jerk.z; + cmd1->yaw += ICs_.yaw() + cmd2->yaw; + + if(!generator_1_.status() || !generator_2_.status()) { - cmd1->header.stamp = ros::Time::now(); - cmd1->header.frame_id = msg->header.frame_id; - cmd1->position.x += ICs_.pos()(0) + cmd2->position.x; - cmd1->position.y += ICs_.pos()(1) + cmd2->position.y; - cmd1->position.z += ICs_.pos()(2) + cmd2->position.z; - cmd1->velocity.x += cmd2->velocity.x; - cmd1->velocity.y += cmd2->velocity.y; - cmd1->velocity.z += cmd2->velocity.z; - cmd1->acceleration.x += cmd2->acceleration.x; - cmd1->acceleration.y += cmd2->acceleration.y; - cmd1->acceleration.z += cmd2->acceleration.z; - cmd1->jerk.x += cmd2->jerk.x; - cmd1->jerk.y += cmd2->jerk.y; - cmd1->jerk.z += cmd2->jerk.z; - cmd1->yaw += ICs_.yaw() + cmd2->yaw; - - // Publish feedback and compute distance traveled - if(!generator_1_.status() || !generator_2_.status()) - { - kr_tracker_msgs::LissajousAdderFeedback feedback; - feedback.time_to_completion = std::max(generator_1_.timeRemaining(), generator_2_.timeRemaining()); - tracker_server_->publishFeedback(feedback); - - Eigen::Vector3d position_current = - Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - distance_traveled_ += (position_current - position_last_).norm(); - position_last_ = position_current; - } - else if(tracker_server_->isActive()) + if(current_goal_handle_ && current_goal_handle_->is_active()) { - kr_tracker_msgs::LissajousAdderResult result; - result.x = msg->pose.pose.position.x; - result.y = msg->pose.pose.position.y; - result.z = msg->pose.pose.position.z; - result.yaw = ICs_.yaw(); // TODO: Change this to the yaw from msg - result.duration = std::max(generator_1_.timeElapsed(), generator_2_.timeElapsed()); - result.length = distance_traveled_; - tracker_server_->setSucceeded(result); + auto feedback = std::make_shared(); + feedback->time_to_completion = std::max(generator_1_.timeRemaining(), generator_2_.timeRemaining()); + current_goal_handle_->publish_feedback(feedback); } - return cmd1; + + const Eigen::Vector3d position_current(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); + distance_traveled_ += (position_current - position_last_).norm(); + position_last_ = position_current; + } + else if(current_goal_handle_ && current_goal_handle_->is_active()) + { + auto result = std::make_shared(); + result->x = msg->pose.pose.position.x; + result->y = msg->pose.pose.position.y; + result->z = msg->pose.pose.position.z; + result->yaw = ICs_.yaw(); + result->duration = std::max(generator_1_.timeElapsed(), generator_2_.timeElapsed()); + result->length = distance_traveled_; + current_goal_handle_->succeed(result); + + generator_1_.deactivate(); + generator_2_.deactivate(); + active_ = false; + current_goal_handle_.reset(); } + + return cmd1; } -uint8_t LissajousAdder::status() const +uint8_t LissajousAdder::status() { - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + std::lock_guard lock(mutex_); + if(active_ && current_goal_handle_ && current_goal_handle_->is_active()) + { + return static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE); + } + return static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); } -void LissajousAdder::goal_callback(void) +rclcpp_action::GoalResponse LissajousAdder::goal_callback( + const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - if(generator_1_.goalIsSet() || generator_2_.goalIsSet()) + (void)uuid; + if(goal->period[0] <= 0.0 || goal->period[1] <= 0.0) { - return; + RCLCPP_WARN(logger_, "Rejecting LissajousAdder goal with non-positive period"); + return rclcpp_action::GoalResponse::REJECT; } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} - kr_tracker_msgs::LissajousAdderGoal::ConstPtr msg = tracker_server_->acceptNewGoal(); +rclcpp_action::CancelResponse LissajousAdder::cancel_callback( + const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); - if(tracker_server_->isPreemptRequested()) + if(current_goal_handle_ == goal_handle) { - tracker_server_->setPreempted(); - return; + auto result = std::make_shared(); + result->duration = std::max(generator_1_.timeElapsed(), generator_2_.timeElapsed()); + result->length = distance_traveled_; + goal_handle->canceled(result); + + generator_1_.deactivate(); + generator_2_.deactivate(); + active_ = false; + traj_start_set_ = false; + current_goal_handle_.reset(); + return rclcpp_action::CancelResponse::ACCEPT; } - traj_start_set_ = false; - generator_1_.setParams(msg, 0); - generator_2_.setParams(msg, 1); - distance_traveled_ = 0; - generator_1_.activate(); - generator_2_.activate(); + return rclcpp_action::CancelResponse::REJECT; } -void LissajousAdder::preempt_callback(void) +void LissajousAdder::handle_accepted_callback(const std::shared_ptr goal_handle) { - if(tracker_server_->isActive()) - { - tracker_server_->setAborted(); - } - else + std::lock_guard lock(mutex_); + + if(current_goal_handle_ && current_goal_handle_->is_active()) { - tracker_server_->setPreempted(); + auto result = std::make_shared(); + result->duration = std::max(generator_1_.timeElapsed(), generator_2_.timeElapsed()); + result->length = distance_traveled_; + current_goal_handle_->abort(result); } + current_goal_handle_ = goal_handle; traj_start_set_ = false; - generator_1_.deactivate(); - generator_2_.deactivate(); + distance_traveled_ = 0.0; + generator_1_.setParams(goal_handle->get_goal(), 0); + generator_2_.setParams(goal_handle->get_goal(), 1); + active_ = false; } -#include PLUGINLIB_EXPORT_CLASS(LissajousAdder, kr_trackers_manager::Tracker); diff --git a/trackers/kr_trackers/src/lissajous_generator.cpp b/trackers/kr_trackers/src/lissajous_generator.cpp index f272c7df..0601ec8d 100644 --- a/trackers/kr_trackers/src/lissajous_generator.cpp +++ b/trackers/kr_trackers/src/lissajous_generator.cpp @@ -1,7 +1,5 @@ -// TODO: convert to ros2 compatible format - -#include -#include +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "kr_trackers/lissajous_generator.h" #include #include @@ -12,9 +10,11 @@ LissajousGenerator::LissajousGenerator() active_ = false; goal_reached_ = false; goal_set_ = false; + clock_ = std::make_shared(RCL_ROS_TIME); + start_time_ = clock_->now(); } -void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousTrackerGoal::ConstPtr &msg) +void LissajousGenerator::setParams(const std::shared_ptr &msg) { x_amp_ = msg->x_amp; y_amp_ = msg->y_amp; @@ -56,7 +56,8 @@ void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousTrackerGoal:: goal_reached_ = false; } -void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousAdderGoal::ConstPtr &msg, int num) +void LissajousGenerator::setParams(const std::shared_ptr &msg, + int num) { x_amp_ = msg->x_amp[num]; y_amp_ = msg->y_amp[num]; @@ -98,20 +99,19 @@ void LissajousGenerator::setParams(const kr_tracker_msgs::LissajousAdderGoal::Co goal_reached_ = false; } -const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void) +kr_mav_msgs::msg::PositionCommand::SharedPtr LissajousGenerator::getPositionCmd() { if(!active_) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::SharedPtr(); } // Set gains - kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand); + auto cmd = std::make_shared(); // Get elapsed time - ros::Time current_time = ros::Time::now(); - ros::Duration elapsed_time = current_time - start_time_; - double t = elapsed_time.toSec(); + const rclcpp::Time current_time = clock_->now(); + const double t = (current_time - start_time_).seconds(); double t2 = t * t; double t3 = t2 * t; double t4 = t3 * t; @@ -171,15 +171,15 @@ const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void) double T = period_; double T2 = T * T; double T3 = T2 * T; - pos(0) = x_amp_ * (1 - std::cos(2 * M_PI * x_num_periods_ * s / T)); + pos(0) = x_amp_ * std::sin(2 * M_PI * x_num_periods_ * s / T); pos(1) = y_amp_ * std::sin(2 * M_PI * y_num_periods_ * s / T); pos(2) = z_amp_ * std::sin(2 * M_PI * z_num_periods_ * s / T); - vel(0) = x_amp_ * 2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sdot / T; + vel(0) = x_amp_ * 2 * M_PI * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) * sdot / T; vel(1) = y_amp_ * 2 * M_PI * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sdot / T; vel(2) = z_amp_ * 2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sdot / T; - acc(0) = x_amp_ * (4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) * - sdot * sdot / T2 + - 2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sddot / T); + acc(0) = x_amp_ * (-4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * + std::sin(2 * M_PI * x_num_periods_ * s / T) * sdot * sdot / T2 + + 2 * M_PI * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) * sddot / T); acc(1) = y_amp_ * (-4 * M_PI * M_PI * y_num_periods_ * y_num_periods_ * std::sin(2 * M_PI * y_num_periods_ * s / T) * sdot * sdot / T2 + 2 * M_PI * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sddot / T); @@ -187,10 +187,10 @@ const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void) std::sin(2 * M_PI * z_num_periods_ * s / T) * sdot * sdot / T2 + 2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sddot / T); jrk(0) = x_amp_ * (-8 * M_PI * M_PI * M_PI * x_num_periods_ * x_num_periods_ * x_num_periods_ * - std::sin(2 * M_PI * x_num_periods_ * s / T) * sdot * sdot * sdot / T3 + - 4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) * - sdot * sddot / T2 + - 2 * M_PI * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * sdddot / T); + std::cos(2 * M_PI * x_num_periods_ * s / T) * sdot * sdot * sdot / T3 - + 4 * M_PI * M_PI * x_num_periods_ * x_num_periods_ * std::sin(2 * M_PI * x_num_periods_ * s / T) * + sdot * sddot / T2 + + 2 * M_PI * x_num_periods_ * std::cos(2 * M_PI * x_num_periods_ * s / T) * sdddot / T); jrk(1) = y_amp_ * (-8 * M_PI * M_PI * M_PI * y_num_periods_ * y_num_periods_ * y_num_periods_ * std::cos(2 * M_PI * y_num_periods_ * s / T) * sdot * sdot * sdot / T3 - 4 * M_PI * M_PI * y_num_periods_ * y_num_periods_ * std::sin(2 * M_PI * y_num_periods_ * s / T) * @@ -201,8 +201,8 @@ const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void) 4 * M_PI * M_PI * z_num_periods_ * z_num_periods_ * std::sin(2 * M_PI * z_num_periods_ * s / T) * sdot * sddot / T2 + 2 * M_PI * z_num_periods_ * std::cos(2 * M_PI * z_num_periods_ * s / T) * sdddot / T); - yaw = yaw_amp_ * (1 - std::cos(2 * M_PI * yaw_num_periods_ * s / T)); - yaw_dot = yaw_amp_ * 2 * M_PI * yaw_num_periods_ * std::sin(2 * M_PI * yaw_num_periods_ * s / T) * sdot / T; + yaw = yaw_amp_ * std::sin(2 * M_PI * yaw_num_periods_ * s / T); + yaw_dot = yaw_amp_ * 2 * M_PI * yaw_num_periods_ * std::cos(2 * M_PI * yaw_num_periods_ * s / T) * sdot / T; cmd->position.x = pos(0), cmd->position.y = pos(1), cmd->position.z = pos(2); cmd->velocity.x = vel(0), cmd->velocity.y = vel(1), cmd->velocity.z = vel(2); cmd->acceleration.x = acc(0), cmd->acceleration.y = acc(1), cmd->acceleration.z = acc(2); @@ -213,7 +213,7 @@ const kr_mav_msgs::PositionCommand::Ptr LissajousGenerator::getPositionCmd(void) return cmd; } -void LissajousGenerator::generatePath(nav_msgs::Path &path, geometry_msgs::Point &initial_pt, double dt) +void LissajousGenerator::generatePath(nav_msgs::msg::Path &path, const geometry_msgs::msg::Point &initial_pt, double dt) { if(goal_set_) { @@ -222,8 +222,8 @@ void LissajousGenerator::generatePath(nav_msgs::Path &path, geometry_msgs::Point while(s < period_) { - geometry_msgs::PoseStamped ps; - ps.pose.position.x = x_amp_ * (1 - std::cos(2 * M_PI * x_num_periods_ * s / T)) + initial_pt.x; + geometry_msgs::msg::PoseStamped ps; + ps.pose.position.x = x_amp_ * std::sin(2 * M_PI * x_num_periods_ * s / T) + initial_pt.x; ps.pose.position.y = y_amp_ * std::sin(2 * M_PI * y_num_periods_ * s / T) + initial_pt.y; ps.pose.position.z = z_amp_ * std::sin(2 * M_PI * z_num_periods_ * s / T) + initial_pt.z; @@ -233,46 +233,46 @@ void LissajousGenerator::generatePath(nav_msgs::Path &path, geometry_msgs::Point } } -bool LissajousGenerator::activate(void) +bool LissajousGenerator::activate() { if(goal_set_) { active_ = true; - start_time_ = ros::Time::now(); + start_time_ = clock_->now(); } return active_; } -void LissajousGenerator::deactivate(void) +void LissajousGenerator::deactivate() { goal_set_ = false; active_ = false; } -bool LissajousGenerator::isActive(void) +bool LissajousGenerator::isActive() const { return active_; } -bool LissajousGenerator::goalIsSet(void) +bool LissajousGenerator::goalIsSet() const { return goal_set_; } bool LissajousGenerator::status() const { - return goal_reached_ ? kr_tracker_msgs::TrackerStatus::SUCCEEDED : kr_tracker_msgs::TrackerStatus::ACTIVE; + return goal_reached_ ? kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED : kr_tracker_msgs::msg::TrackerStatus::ACTIVE; } -float LissajousGenerator::timeRemaining(void) +float LissajousGenerator::timeRemaining() const { - ros::Time t_now = ros::Time::now(); - float time_elapsed = (t_now - start_time_).toSec(); - return total_time_ - time_elapsed; + const rclcpp::Time t_now = clock_->now(); + const float time_elapsed = static_cast((t_now - start_time_).seconds()); + return std::max(0.0f, static_cast(total_time_) - time_elapsed); } -float LissajousGenerator::timeElapsed(void) +float LissajousGenerator::timeElapsed() const { - ros::Time t_now = ros::Time::now(); - return (t_now - start_time_).toSec(); + const rclcpp::Time t_now = clock_->now(); + return static_cast((t_now - start_time_).seconds()); } diff --git a/trackers/kr_trackers/src/lissajous_tracker_server.cpp b/trackers/kr_trackers/src/lissajous_tracker_server.cpp index 69da3c4c..60478602 100644 --- a/trackers/kr_trackers/src/lissajous_tracker_server.cpp +++ b/trackers/kr_trackers/src/lissajous_tracker_server.cpp @@ -1,90 +1,122 @@ -// TODO: convert to ros2 compatible format - -#include -#include -#include -#include -#include -#include -#include +#include "kr_trackers/Tracker.hpp" +#include "kr_trackers/initial_conditions.hpp" +#include "kr_trackers/lissajous_generator.h" + +#include "geometry_msgs/msg/point.hpp" +#include "kr_mav_msgs/msg/position_command.hpp" +#include "kr_tracker_msgs/action/lissajous_tracker.hpp" +#include "kr_tracker_msgs/msg/tracker_status.hpp" +#include "nav_msgs/msg/path.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include -#include #include +#include class LissajousTracker : public kr_trackers_manager::Tracker { public: - LissajousTracker(void); - void Initialize(const ros::NodeHandle &nh); - bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd); - void Deactivate(void); + LissajousTracker() = default; - kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg); - uint8_t status() const; + void Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) override; + bool Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) override; + void Deactivate() override; - private: - void goal_callback(void); - void preempt_callback(void); + kr_mav_msgs::msg::PositionCommand::ConstSharedPtr update(const nav_msgs::msg::Odometry::SharedPtr msg) override; + uint8_t status() override; - typedef actionlib::SimpleActionServer ServerType; - std::shared_ptr tracker_server_; - ros::Publisher path_pub_; + private: + using LissajousTrackerAction = kr_tracker_msgs::action::LissajousTracker; + using LissajousTrackerGoalHandle = rclcpp_action::ServerGoalHandle; + + rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID &uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); + void handle_accepted_callback(const std::shared_ptr goal_handle); + + rclcpp::Logger logger_{rclcpp::get_logger("trackers_manager")}; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp_action::Server::SharedPtr tracker_server_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + std::shared_ptr current_goal_handle_; + std::recursive_mutex mutex_; InitialConditions ICs_; LissajousGenerator generator_; - double distance_traveled_; - Eigen::Vector3d position_last_; - bool traj_start_set_; - std::string frame_id_; + double distance_traveled_{0.0}; + Eigen::Vector3d position_last_{Eigen::Vector3d::Zero()}; + bool traj_start_set_{false}; + bool active_{false}; + std::string frame_id_{"odom"}; }; -LissajousTracker::LissajousTracker(void) : traj_start_set_(false) {} - -void LissajousTracker::Initialize(const ros::NodeHandle &nh) +void LissajousTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) { - ros::NodeHandle priv_nh(nh, "lissajous_tracker"); - priv_nh.param("frame_id", frame_id_, "world"); - path_pub_ = priv_nh.advertise("lissajous_path", 1); - - tracker_server_ = std::shared_ptr(new ServerType(priv_nh, "LissajousTracker", false)); - tracker_server_->registerGoalCallback(boost::bind(&LissajousTracker::goal_callback, this)); - tracker_server_->registerPreemptCallback(boost::bind(&LissajousTracker::preempt_callback, this)); - tracker_server_->start(); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + node->declare_parameter("lissajous_tracker/frame_id", "odom"); + frame_id_ = node->get_parameter("lissajous_tracker/frame_id").as_string(); + + path_pub_ = node->create_publisher("~/lissajous_tracker/lissajous_path", 1); + + cb_group_ = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + tracker_server_ = rclcpp_action::create_server( + node, + "~/lissajous_tracker/LissajousTracker", + std::bind(&LissajousTracker::goal_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&LissajousTracker::cancel_callback, this, std::placeholders::_1), + std::bind(&LissajousTracker::handle_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), cb_group_); + + RCLCPP_INFO(logger_, "Initialized LissajousTracker"); } -bool LissajousTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd) +bool LissajousTracker::Activate(const kr_mav_msgs::msg::PositionCommand::ConstSharedPtr cmd) { - // Only allow activation if a goal has been set - if(generator_.goalIsSet()) + (void)cmd; + std::lock_guard lock(mutex_); + + if(generator_.goalIsSet() && current_goal_handle_ && current_goal_handle_->is_active()) { - if(!tracker_server_->isActive()) - { - ROS_WARN("LissajousTracker::Activate: goal_set is true but action server has no active goal - not activating."); - return false; - } - return generator_.activate(); + active_ = generator_.activate(); + return active_; } + + active_ = false; return false; } -void LissajousTracker::Deactivate(void) +void LissajousTracker::Deactivate() { - if(tracker_server_->isActive()) + std::lock_guard lock(mutex_); + + if(current_goal_handle_ && current_goal_handle_->is_active()) { - ROS_WARN("LissajousTracker deactivated tracker prior to reaching goal"); - tracker_server_->setAborted(); + auto result = std::make_shared(); + result->duration = generator_.timeElapsed(); + result->length = distance_traveled_; + current_goal_handle_->abort(result); + current_goal_handle_.reset(); } + ICs_.reset(); generator_.deactivate(); traj_start_set_ = false; + active_ = false; } -kr_mav_msgs::PositionCommand::ConstPtr LissajousTracker::update(const nav_msgs::Odometry::ConstPtr &msg) +kr_mav_msgs::msg::PositionCommand::ConstSharedPtr LissajousTracker::update(const nav_msgs::msg::Odometry::SharedPtr msg) { - if(!generator_.isActive()) + std::lock_guard lock(mutex_); + + if(!active_ || !generator_.isActive()) { - return kr_mav_msgs::PositionCommand::Ptr(); + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } if(!traj_start_set_) @@ -93,111 +125,124 @@ kr_mav_msgs::PositionCommand::ConstPtr LissajousTracker::update(const nav_msgs:: ICs_.set_from_odom(msg); position_last_ = Eigen::Vector3d(ICs_.pos()(0), ICs_.pos()(1), ICs_.pos()(2)); - // Generate path for visualizing - geometry_msgs::Point initial_pt; + geometry_msgs::msg::Point initial_pt; initial_pt.x = ICs_.pos()(0); initial_pt.y = ICs_.pos()(1); initial_pt.z = ICs_.pos()(2); - double dt = 0.1; - nav_msgs::Path path; + + nav_msgs::msg::Path path; path.header.frame_id = frame_id_; - path.header.stamp = ros::Time::now(); - generator_.generatePath(path, initial_pt, dt); - path_pub_.publish(path); + path.header.stamp = clock_->now(); + generator_.generatePath(path, initial_pt, 0.1); + path_pub_->publish(path); } - // Set gains - kr_mav_msgs::PositionCommand::Ptr cmd = generator_.getPositionCmd(); - if(cmd == NULL) + auto cmd = generator_.getPositionCmd(); + if(!cmd) { - return cmd; + return kr_mav_msgs::msg::PositionCommand::ConstSharedPtr(); } - else + + cmd->header.stamp = clock_->now(); + cmd->header.frame_id = msg->header.frame_id; + cmd->position.x += ICs_.pos()(0); + cmd->position.y += ICs_.pos()(1); + cmd->position.z += ICs_.pos()(2); + cmd->yaw += ICs_.yaw(); + + if(!generator_.status()) { - cmd->header.stamp = ros::Time::now(); - cmd->header.frame_id = msg->header.frame_id; - cmd->position.x += ICs_.pos()(0); - cmd->position.y += ICs_.pos()(1); - cmd->position.z += ICs_.pos()(2); - cmd->yaw += ICs_.yaw(); - - // Publish feedback and compute distance traveled - if(!generator_.status()) + if(current_goal_handle_ && current_goal_handle_->is_active()) { - kr_tracker_msgs::LissajousTrackerFeedback feedback; - feedback.time_to_completion = generator_.timeRemaining(); - tracker_server_->publishFeedback(feedback); - - Eigen::Vector3d position_current = - Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - distance_traveled_ += (position_current - position_last_).norm(); - position_last_ = position_current; + auto feedback = std::make_shared(); + feedback->time_to_completion = generator_.timeRemaining(); + current_goal_handle_->publish_feedback(feedback); } - else if(tracker_server_->isActive()) - { - kr_tracker_msgs::LissajousTrackerResult result; - result.x = msg->pose.pose.position.x; - result.y = msg->pose.pose.position.y; - result.z = msg->pose.pose.position.z; - result.yaw = ICs_.yaw(); // TODO: Change this to the yaw from msg - result.duration = generator_.timeElapsed(); - result.length = distance_traveled_; - tracker_server_->setSucceeded(result); - } - return cmd; + + const Eigen::Vector3d position_current(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); + distance_traveled_ += (position_current - position_last_).norm(); + position_last_ = position_current; } + else if(current_goal_handle_ && current_goal_handle_->is_active()) + { + auto result = std::make_shared(); + result->x = msg->pose.pose.position.x; + result->y = msg->pose.pose.position.y; + result->z = msg->pose.pose.position.z; + result->yaw = ICs_.yaw(); + result->duration = generator_.timeElapsed(); + result->length = distance_traveled_; + current_goal_handle_->succeed(result); + + generator_.deactivate(); + active_ = false; + current_goal_handle_.reset(); + } + + return cmd; } -uint8_t LissajousTracker::status() const +uint8_t LissajousTracker::status() { - return tracker_server_->isActive() ? static_cast(kr_tracker_msgs::TrackerStatus::ACTIVE) : - static_cast(kr_tracker_msgs::TrackerStatus::SUCCEEDED); + std::lock_guard lock(mutex_); + if(active_ && current_goal_handle_ && current_goal_handle_->is_active()) + { + return static_cast(kr_tracker_msgs::msg::TrackerStatus::ACTIVE); + } + return static_cast(kr_tracker_msgs::msg::TrackerStatus::SUCCEEDED); } -void LissajousTracker::goal_callback(void) +rclcpp_action::GoalResponse LissajousTracker::goal_callback( + const rclcpp_action::GoalUUID &uuid, std::shared_ptr goal) { - // If another goal is already active, cancel that goal - // and track this one instead. - if(tracker_server_->isActive()) + (void)uuid; + if(goal->period <= 0.0 || goal->num_cycles <= 0.0) { - ROS_INFO("Previous LissajousTracker goal aborted."); - tracker_server_->setAborted(); - generator_.deactivate(); + RCLCPP_WARN(logger_, "Rejecting Lissajous goal with non-positive period/num_cycles"); + return rclcpp_action::GoalResponse::REJECT; } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} - kr_tracker_msgs::LissajousTrackerGoal::ConstPtr msg = tracker_server_->acceptNewGoal(); +rclcpp_action::CancelResponse LissajousTracker::cancel_callback( + const std::shared_ptr goal_handle) +{ + std::lock_guard lock(mutex_); - // If preempt has been requested, then set this goal to preempted - // and make no changes to the tracker state. - if(tracker_server_->isPreemptRequested()) + if(current_goal_handle_ == goal_handle) { - ROS_INFO("LissajousTracker going to goal preempted."); - tracker_server_->setPreempted(); - return; - } + auto result = std::make_shared(); + result->duration = generator_.timeElapsed(); + result->length = distance_traveled_; + goal_handle->canceled(result); - generator_.setParams(msg); + generator_.deactivate(); + active_ = false; + traj_start_set_ = false; + current_goal_handle_.reset(); + return rclcpp_action::CancelResponse::ACCEPT; + } - traj_start_set_ = false; - distance_traveled_ = 0; - generator_.activate(); + return rclcpp_action::CancelResponse::REJECT; } -void LissajousTracker::preempt_callback(void) +void LissajousTracker::handle_accepted_callback(const std::shared_ptr goal_handle) { - ICs_.reset(); - generator_.deactivate(); - traj_start_set_ = false; + std::lock_guard lock(mutex_); - if(tracker_server_->isActive()) - { - tracker_server_->setAborted(); - } - else + if(current_goal_handle_ && current_goal_handle_->is_active()) { - tracker_server_->setPreempted(); + auto result = std::make_shared(); + result->duration = generator_.timeElapsed(); + result->length = distance_traveled_; + current_goal_handle_->abort(result); } + + current_goal_handle_ = goal_handle; + generator_.setParams(goal_handle->get_goal()); + traj_start_set_ = false; + distance_traveled_ = 0.0; + active_ = false; } -#include PLUGINLIB_EXPORT_CLASS(LissajousTracker, kr_trackers_manager::Tracker); diff --git a/trackers/kr_trackers_manager/config/trackers_manager.yaml b/trackers/kr_trackers_manager/config/trackers_manager.yaml index db36a54f..f2d7c393 100644 --- a/trackers/kr_trackers_manager/config/trackers_manager.yaml +++ b/trackers/kr_trackers_manager/config/trackers_manager.yaml @@ -1,6 +1,7 @@ trackers_manager: ros__parameters: - trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker"] + trackers: ["NullTracker", "LineTrackerDistance", "LineTrackerMinJerk", "PolyTracker", "CircleTracker", "LissajousTracker", "LissajousAdder"] + circle_tracker/ramp_up_time: 2.0 line_tracker_distance/default_v_des: 0.5 line_tracker_distance/default_a_des: 0.5 line_tracker_distance/epsilon: 0.1 @@ -8,3 +9,5 @@ trackers_manager: line_tracker_min_jerk/default_a_des: 0.3 line_tracker_min_jerk/default_yaw_v_des: 0.1 line_tracker_min_jerk/default_yaw_a_des: 0.2 + lissajous_tracker/frame_id: odom + lissajous_adder/frame_id: odom From 21f99d574d2ae467c223ea8b612b75f42f277abc Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 5 May 2026 00:34:16 +0000 Subject: [PATCH 2/6] Added circle ramp to gui --- .../include/kr_mav_manager/manager.hpp | 2 +- .../kr_mav_manager/mav_manager_services.hpp | 2 +- kr_mav_manager/src/manager.cpp | 55 ++++++++++++++++++- kr_mav_manager/srv/Circle.srv | 1 + rqt_mav_manager/rqt_mav_manager/__init__.py | 10 +++- .../action/CircleTracker.action | 1 + .../kr_trackers/src/circle_tracker_server.cpp | 36 ++++++++++-- 7 files changed, 97 insertions(+), 10 deletions(-) diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index 2551678e..35f3b487 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -105,7 +105,7 @@ class MAVManager : public rclcpp::Node // Yaw Control bool goToYaw(float); - bool circle(float Ax, float Ay, float T, float duration); + bool circle(float Ax, float Ay, float T, float duration, float ramp_time = 2.0f); // Lissajous Control bool lissajous(float x_amp, float y_amp, float z_amp, float yaw_amp, float x_num_periods, float y_num_periods, diff --git a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp index cc865c55..a3481973 100644 --- a/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp +++ b/kr_mav_manager/include/kr_mav_manager/mav_manager_services.hpp @@ -99,7 +99,7 @@ class MAVManagerServices void circle_cb(const kr_mav_manager::srv::Circle::Request::SharedPtr req, const kr_mav_manager::srv::Circle::Response::SharedPtr res) { - res->success = mav->circle(req->ax, req->ay, req->t, req->duration); + res->success = mav->circle(req->ax, req->ay, req->t, req->duration, req->ramp_time); res->message = "Circling motion"; if(res->success) last_cb_ = "circle"; diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index f9c3444f..507c5e53 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -207,6 +207,23 @@ void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResul void MAVManager::circle_tracker_done_callback(const CircleTrackerGoalHandle::WrappedResult &result) { RCLCPP_INFO(this->get_logger(), "Circle tracking completed after %2.2f seconds", result.result->duration); + + // Send LineTracker goal to hold final position + auto goal = LineTracker::Goal(); + goal.x = pos_(0); + goal.y = pos_(1); + goal.z = pos_(2); + goal.yaw = yaw_; + goal.relative = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + line_tracker_min_jerk_client_->async_send_goal(goal, options); + + if(!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after circle completion"); + } } void MAVManager::lissajous_tracker_done_callback(const LissajousTrackerGoalHandle::WrappedResult &result) @@ -216,6 +233,23 @@ void MAVManager::lissajous_tracker_done_callback(const LissajousTrackerGoalHandl "%2.2f, %2.2f).", result.result->duration, result.result->length, result.result->x, result.result->y, result.result->z, result.result->yaw); + + // Send LineTracker goal to hold final position + auto goal = LineTracker::Goal(); + goal.x = result.result->x; + goal.y = result.result->y; + goal.z = result.result->z; + goal.yaw = result.result->yaw; + goal.relative = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + line_tracker_min_jerk_client_->async_send_goal(goal, options); + + if(!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous completion"); + } } void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::WrappedResult &result) @@ -225,6 +259,23 @@ void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::W "%2.2f, %2.2f).", result.result->duration, result.result->length, result.result->x, result.result->y, result.result->z, result.result->yaw); + + // Send LineTracker goal to hold final position + auto goal = LineTracker::Goal(); + goal.x = result.result->x; + goal.y = result.result->y; + goal.z = result.result->z; + goal.yaw = result.result->yaw; + goal.relative = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + line_tracker_min_jerk_client_->async_send_goal(goal, options); + + if(!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous adder completion"); + } } void MAVManager::poly_tracker_done_callback(const PolyTrackerGoalHandle::WrappedResult &result) @@ -404,6 +455,7 @@ bool MAVManager::land() auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); line_tracker_distance_client_->async_send_goal(goal, options); + std::this_thread::sleep_for(std::chrono::seconds(1)); return this->transition(line_tracker_distance); } @@ -494,7 +546,7 @@ bool MAVManager::goToYaw(float yaw) return this->goTo(pos_(0), pos_(1), pos_(2), yaw); } -bool MAVManager::circle(float Ax, float Ay, float T, float duration) +bool MAVManager::circle(float Ax, float Ay, float T, float duration, float ramp_time) { if(!this->motors() || status_ != FLYING) { @@ -507,6 +559,7 @@ bool MAVManager::circle(float Ax, float Ay, float T, float duration) goal.ay = Ay; goal.t = T; goal.duration = duration; + goal.ramp_time = ramp_time; auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::circle_tracker_done_callback, this, _1); diff --git a/kr_mav_manager/srv/Circle.srv b/kr_mav_manager/srv/Circle.srv index 7b4d64a0..75122be1 100644 --- a/kr_mav_manager/srv/Circle.srv +++ b/kr_mav_manager/srv/Circle.srv @@ -2,6 +2,7 @@ float32 ax float32 ay float32 t float32 duration +float32 ramp_time --- bool success string message \ No newline at end of file diff --git a/rqt_mav_manager/rqt_mav_manager/__init__.py b/rqt_mav_manager/rqt_mav_manager/__init__.py index a73877f5..686465dd 100644 --- a/rqt_mav_manager/rqt_mav_manager/__init__.py +++ b/rqt_mav_manager/rqt_mav_manager/__init__.py @@ -104,6 +104,11 @@ def _build_trajectory_tab(self, parent_widget): self.circle_duration_spinbox.setSingleStep(0.5) self.circle_duration_spinbox.setValue(20.0) + self.circle_ramp_spinbox = QDoubleSpinBox() + self.circle_ramp_spinbox.setRange(0.0, 60.0) + self.circle_ramp_spinbox.setSingleStep(0.1) + self.circle_ramp_spinbox.setValue(2.0) + self.circle_send_button = QPushButton('Start Circle') circle_layout.addWidget(QLabel('Ax'), 0, 0) @@ -114,7 +119,9 @@ def _build_trajectory_tab(self, parent_widget): circle_layout.addWidget(self.circle_period_spinbox, 1, 1) circle_layout.addWidget(QLabel('Duration [s]'), 1, 2) circle_layout.addWidget(self.circle_duration_spinbox, 1, 3) - circle_layout.addWidget(self.circle_send_button, 2, 0, 1, 4) + circle_layout.addWidget(QLabel('ramp_time [s]'), 2, 0) + circle_layout.addWidget(self.circle_ramp_spinbox, 2, 1) + circle_layout.addWidget(self.circle_send_button, 3, 0, 1, 4) lissajous_group = QGroupBox('Lissajous Trajectory') lissajous_layout = QGridLayout(lissajous_group) @@ -430,6 +437,7 @@ def _on_circle_pressed(self): request.ay = self.circle_ay_spinbox.value() request.t = self.circle_period_spinbox.value() request.duration = self.circle_duration_spinbox.value() + request.ramp_time = self.circle_ramp_spinbox.value() circle_topic = '/' + self.robot_name + '/' + self.mav_node_name + '/circle' response = self._call_service(kr_mav_manager.srv.Circle, circle_topic, request) diff --git a/trackers/kr_tracker_msgs/action/CircleTracker.action b/trackers/kr_tracker_msgs/action/CircleTracker.action index b6e14b3f..c90100b6 100644 --- a/trackers/kr_tracker_msgs/action/CircleTracker.action +++ b/trackers/kr_tracker_msgs/action/CircleTracker.action @@ -3,6 +3,7 @@ float64 ax float64 ay float64 t float64 duration +float64 ramp_time --- #result definition # time duration of the trajectory diff --git a/trackers/kr_trackers/src/circle_tracker_server.cpp b/trackers/kr_trackers/src/circle_tracker_server.cpp index 500402bb..cfe83f9f 100644 --- a/trackers/kr_trackers/src/circle_tracker_server.cpp +++ b/trackers/kr_trackers/src/circle_tracker_server.cpp @@ -65,6 +65,7 @@ class CircleTracker : public kr_trackers_manager::Tracker float traj_duration_{0.0f}; float omega_{0.0f}; float ramp_up_time_{2.0f}; + float ramp_down_time_{2.0f}; Eigen::Vector3f offset_pos_{Eigen::Vector3f::Zero()}; Eigen::Vector3f final_pos_{Eigen::Vector3f::Zero()}; @@ -81,6 +82,7 @@ void CircleTracker::Initialize(rclcpp_lifecycle::LifecycleNode::WeakPtr &parent) node->declare_parameter("circle_tracker/ramp_up_time", 2.0); ramp_up_time_ = static_cast(node->get_parameter("circle_tracker/ramp_up_time").as_double()); + ramp_down_time_ = ramp_up_time_; pub_start_ = node->create_publisher("~/circle_tracker/traj_start", 10); pub_end_ = node->create_publisher("~/circle_tracker/traj_end", 10); @@ -179,24 +181,39 @@ kr_mav_msgs::msg::PositionCommand::ConstSharedPtr CircleTracker::update(const na const float t = std::max(0.0f, static_cast((clock_->now() - traj_start_time_).seconds())); const float t_eval = std::min(t, traj_duration_); - const float ramp_t = std::max(0.0f, std::min(ramp_up_time_, traj_duration_)); + const float ramp_t_up = std::max(0.0f, std::min(ramp_up_time_, traj_duration_)); + const float ramp_t_down = std::max(0.0f, std::min(ramp_down_time_, traj_duration_)); float s = 1.0f; float s_dot = 0.0f; float s_ddot = 0.0f; float s_dddot = 0.0f; - if(ramp_t > 1e-4f && t_eval < ramp_t) + if(ramp_t_up > 1e-4f && t_eval < ramp_t_up) { - const float tau = std::max(0.0f, std::min(1.0f, t_eval / ramp_t)); + const float tau = std::max(0.0f, std::min(1.0f, t_eval / ramp_t_up)); const float tau2 = tau * tau; const float tau3 = tau2 * tau; const float tau4 = tau3 * tau; // smooth ramp up s = 10.0f * tau3 - 15.0f * tau4 + 6.0f * tau4 * tau; - s_dot = (30.0f * tau2 - 60.0f * tau3 + 30.0f * tau4) / ramp_t; - s_ddot = (60.0f * tau - 180.0f * tau2 + 120.0f * tau3) / (ramp_t * ramp_t); - s_dddot = (60.0f - 360.0f * tau + 360.0f * tau2) / (ramp_t * ramp_t * ramp_t); + s_dot = (30.0f * tau2 - 60.0f * tau3 + 30.0f * tau4) / ramp_t_up; + s_ddot = (60.0f * tau - 180.0f * tau2 + 120.0f * tau3) / (ramp_t_up * ramp_t_up); + s_dddot = (60.0f - 360.0f * tau + 360.0f * tau2) / (ramp_t_up * ramp_t_up * ramp_t_up); + } + else if(ramp_t_down > 1e-4f && t_eval > (traj_duration_ - ramp_t_down)) + { + const float t_remain = traj_duration_ - t_eval; + const float tau = std::max(0.0f, std::min(1.0f, t_remain / ramp_t_down)); + const float tau2 = tau * tau; + const float tau3 = tau2 * tau; + const float tau4 = tau3 * tau; + + // smooth ramp down: s goes from 1.0 -> 0.0 + s = 10.0f * tau3 - 15.0f * tau4 + 6.0f * tau4 * tau; + s_dot = -(30.0f * tau2 - 60.0f * tau3 + 30.0f * tau4) / ramp_t_down; + s_ddot = -(60.0f * tau - 180.0f * tau2 + 120.0f * tau3) / (ramp_t_down * ramp_t_down); + s_dddot = -(60.0f - 360.0f * tau + 360.0f * tau2) / (ramp_t_down * ramp_t_down * ramp_t_down); } const float theta = omega_ * t_eval; @@ -360,6 +377,13 @@ void CircleTracker::handle_accepted_callback(const std::shared_ptr(goal->duration); omega_ = static_cast(2.0 * M_PI / period_); + // Use ramp_time from goal if provided (> 0), otherwise keep the parameter default + if(goal->ramp_time > 0.0) + { + ramp_up_time_ = static_cast(goal->ramp_time); + ramp_down_time_ = ramp_up_time_; + } + current_goal_handle_ = goal_handle; traj_started_ = false; traj_completed_ = false; From dd4745a7af1ebf3f7cf5b20f0d801538fb8c0cac Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 5 May 2026 00:51:01 +0000 Subject: [PATCH 3/6] Fixed land --- kr_mav_manager/src/manager.cpp | 66 ++++++++++++++----- .../kr_trackers/src/circle_tracker_server.cpp | 2 +- 2 files changed, 50 insertions(+), 18 deletions(-) diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 507c5e53..bea081ee 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -218,12 +218,19 @@ void MAVManager::circle_tracker_done_callback(const CircleTrackerGoalHandle::Wra auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LineTrackerMinJerk goal was rejected after circle completion"); + return; + } + if (!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after circle completion"); + } + }; line_tracker_min_jerk_client_->async_send_goal(goal, options); - - if(!this->transition(line_tracker_min_jerk)) - { - RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after circle completion"); - } } void MAVManager::lissajous_tracker_done_callback(const LissajousTrackerGoalHandle::WrappedResult &result) @@ -244,12 +251,19 @@ void MAVManager::lissajous_tracker_done_callback(const LissajousTrackerGoalHandl auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LineTrackerMinJerk goal was rejected after lissajous completion"); + return; + } + if (!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous completion"); + } + }; line_tracker_min_jerk_client_->async_send_goal(goal, options); - - if(!this->transition(line_tracker_min_jerk)) - { - RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous completion"); - } } void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::WrappedResult &result) @@ -270,12 +284,19 @@ void MAVManager::lissajous_adder_done_callback(const LissajousAdderGoalHandle::W auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LineTrackerMinJerk goal was rejected after lissajous adder completion"); + return; + } + if (!this->transition(line_tracker_min_jerk)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous adder completion"); + } + }; line_tracker_min_jerk_client_->async_send_goal(goal, options); - - if(!this->transition(line_tracker_min_jerk)) - { - RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerMinJerk after lissajous adder completion"); - } } void MAVManager::poly_tracker_done_callback(const PolyTrackerGoalHandle::WrappedResult &result) @@ -454,10 +475,21 @@ bool MAVManager::land() std::cout << " landing at " << goal.x << " " << goal.y << " " << goal.z << std::endl; auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); + options.goal_response_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) + { + RCLCPP_WARN(this->get_logger(), "LineTrackerDistance goal was rejected for landing"); + return; + } + if (!this->transition(line_tracker_distance)) + { + RCLCPP_WARN(this->get_logger(), "Failed to transition to LineTrackerDistance for landing"); + } + }; line_tracker_distance_client_->async_send_goal(goal, options); - std::this_thread::sleep_for(std::chrono::seconds(1)); - return this->transition(line_tracker_distance); + return true; } bool MAVManager::goTo(float x, float y, float z, float yaw, float v_des, float a_des, bool relative) diff --git a/trackers/kr_trackers/src/circle_tracker_server.cpp b/trackers/kr_trackers/src/circle_tracker_server.cpp index cfe83f9f..5ee5a594 100644 --- a/trackers/kr_trackers/src/circle_tracker_server.cpp +++ b/trackers/kr_trackers/src/circle_tracker_server.cpp @@ -64,7 +64,7 @@ class CircleTracker : public kr_trackers_manager::Tracker float period_{1.0f}; float traj_duration_{0.0f}; float omega_{0.0f}; - float ramp_up_time_{2.0f}; + float ramp_up_time_{2.0f}; float ramp_down_time_{2.0f}; Eigen::Vector3f offset_pos_{Eigen::Vector3f::Zero()}; From b9d64e2ad249eab8cc79a4d6aa253e79f68241a3 Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 5 May 2026 17:49:32 +0000 Subject: [PATCH 4/6] Added yaw correction to so3control --- .../src/so3_control_component.cpp | 20 ++++++++++++++++--- kr_mav_manager/src/manager.cpp | 6 +++++- .../src/line_tracker_distance_server.cpp | 2 -- .../src/line_tracker_min_jerk_server.cpp | 2 -- 4 files changed, 22 insertions(+), 8 deletions(-) diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index f16a9ef6..23c70d6d 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -79,9 +79,23 @@ void SO3ControlComponent::publishSO3Command() const Eigen::Vector3f &force = controller_.getComputedForce(); const Eigen::Quaternionf &orientation = controller_.getComputedOrientation(); - const Eigen::Vector3f &ang_vel = controller_.getComputedAngularVelocity(); - - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "desired_yaw_rate: %2.3f", ang_vel(2)); + const Eigen::Vector3f &ang_vel_ff = controller_.getComputedAngularVelocity(); + + // Close the yaw attitude loop: SO3Control only computes feedforward angular velocity + // (R_des^T * R_des_dot), which is ~0 when des_yaw_dot=0. Add proportional yaw error + // feedback here to correct for this. + float e_yaw = des_yaw_ - current_yaw_; + if(e_yaw > static_cast(M_PI)) + e_yaw -= 2.0f * static_cast(M_PI); + else if(e_yaw < -static_cast(M_PI)) + e_yaw += 2.0f * static_cast(M_PI); + + Eigen::Vector3f ang_vel = ang_vel_ff; + ang_vel(2) += kr_[2] * e_yaw; // kr_[2] is rot_z, proportional yaw attitude gain + + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "yaw_rate: %2.3f (ff: %2.3f, fb: %2.3f, e_yaw: %2.3f)", + ang_vel(2), ang_vel_ff(2), kr_[2] * e_yaw, e_yaw); auto so3_command = std::make_unique(); so3_command->header.stamp = this->now(); diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index bea081ee..667e9ccc 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -398,12 +398,16 @@ bool MAVManager::takeoff() return false; } + double takeoff_vel = 0.2f; + double takeoff_accel = 0.1f; + RCLCPP_INFO(this->get_logger(), "Initiating launch sequence..."); auto goal = LineTracker::Goal(); goal.z = takeoff_height_; goal.relative = true; - + goal.v_des = takeoff_vel; + goal.a_des = takeoff_accel; auto options = rclcpp_action::Client::SendGoalOptions(); options.result_callback = std::bind(&MAVManager::tracker_done_callback, this, _1); line_tracker_distance_client_->async_send_goal(goal, options); diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index 9f194f76..f5502732 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -393,8 +393,6 @@ void LineTrackerDistance::handle_accepted_callback(const std::shared_ptr lock(mutex_); current_goal_handle_ = goal_handle; - - RCLCPP_INFO_STREAM(logger_, "Out of HAC: "); } uint8_t LineTrackerDistance::status() diff --git a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp index ca266af0..55623fe2 100644 --- a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp @@ -493,8 +493,6 @@ void LineTrackerMinJerk::handle_accepted_callback(const std::shared_ptrget_goal(); } From 206a367ba45f4c3980ffcb2e39d1e8f0db20fb75 Mon Sep 17 00:00:00 2001 From: Dexter Date: Thu, 7 May 2026 21:01:19 +0000 Subject: [PATCH 5/6] Updated polytrack update traj --- .../src/crsf/crsf_bridge.cpp | 8 +++--- .../src/so3_control_component.cpp | 12 ++++---- trackers/kr_trackers/src/poly_tracker.cpp | 28 +++++++++++++++---- 3 files changed, 32 insertions(+), 16 deletions(-) diff --git a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp index e19b6923..29b3e822 100644 --- a/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp +++ b/interfaces/kr_betaflight_interface/src/crsf/crsf_bridge.cpp @@ -348,10 +348,10 @@ CrsfMsg CrsfBridge::generateCrsfMessageFromSO3Command(const kr_mav_msgs::msg::SO // remap throttle (1000 to 2000) to crsf (kMinCmd to kMaxCmd) uint16_t throttle_cmd = round(((throttle - 1000) / 1000) * (CrsfMsg::kMaxCmd - CrsfMsg::kMinCmd) + CrsfMsg::kMinCmd); - RCLCPP_INFO_THROTTLE( - logger_, *node_->get_clock(), 1000, - "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d", - thrust, throttle, throttle_cmd); +// RCLCPP_INFO_THROTTLE( +// logger_, *node_->get_clock(), 1000, +// "AUTONOMOUS MODE: thrust: %f throttle: %f throttle_cmd: %d", +// thrust, throttle, throttle_cmd); crsf_msg.setThrottleCommand(throttle_cmd); // convert quaternion to euler diff --git a/kr_mav_controllers/src/so3_control_component.cpp b/kr_mav_controllers/src/so3_control_component.cpp index 23c70d6d..0c3d4b52 100644 --- a/kr_mav_controllers/src/so3_control_component.cpp +++ b/kr_mav_controllers/src/so3_control_component.cpp @@ -69,11 +69,11 @@ void SO3ControlComponent::publishSO3Command() // RCLCPP_INFO_STREAM(this->get_logger(), "des_pos_: " << des_acc_); // RCLCPP_INFO_STREAM(this->get_logger(), "des_jrk_: " << des_jrk_); // RCLCPP_INFO_STREAM(this->get_logger(), "des_yaw_: " << des_yaw_); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "des_pos_: %2.3f, %2.3f, %2.3f", des_pos_(0), des_pos_(1), des_pos_(2)); - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "des_yaw_: %2.3f", des_yaw_); +// RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "des_pos_: %2.3f, %2.3f, %2.3f", des_pos_(0), des_pos_(1), des_pos_(2)); +// RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "des_yaw_: %2.3f", des_yaw_); // print current yaw - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "current_yaw_: %2.3f", current_yaw_); +// RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "current_yaw_: %2.3f", current_yaw_); controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_jrk_, des_yaw_, des_yaw_dot_, kx_, kv_, ki, kib); @@ -93,9 +93,9 @@ void SO3ControlComponent::publishSO3Command() Eigen::Vector3f ang_vel = ang_vel_ff; ang_vel(2) += kr_[2] * e_yaw; // kr_[2] is rot_z, proportional yaw attitude gain - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "yaw_rate: %2.3f (ff: %2.3f, fb: %2.3f, e_yaw: %2.3f)", - ang_vel(2), ang_vel_ff(2), kr_[2] * e_yaw, e_yaw); +// RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, +// "yaw_rate: %2.3f (ff: %2.3f, fb: %2.3f, e_yaw: %2.3f)", +// ang_vel(2), ang_vel_ff(2), kr_[2] * e_yaw, e_yaw); auto so3_command = std::make_unique(); so3_command->header.stamp = this->now(); diff --git a/trackers/kr_trackers/src/poly_tracker.cpp b/trackers/kr_trackers/src/poly_tracker.cpp index b9762025..df42d8c7 100644 --- a/trackers/kr_trackers/src/poly_tracker.cpp +++ b/trackers/kr_trackers/src/poly_tracker.cpp @@ -355,29 +355,45 @@ rclcpp_action::GoalResponse PolyTracker::goal_callback(const rclcpp_action::Goal { (void)uuid; std::lock_guard lock(mutex_); - // If another goal is already active, reject new goal unless we want to preempt + // If another goal is already active, we will preempt it by accepting the new goal if(current_goal_handle_ && current_goal_handle_->is_active()) { - RCLCPP_INFO(logger_, "PolyTracker: rejecting new goal because another is active"); - return rclcpp_action::GoalResponse::REJECT; + RCLCPP_INFO(logger_, "PolyTracker: accepting new goal and will preempt current goal"); } - // Accept all other goals + // Accept all goals (preemption will be handled in handle_accepted_callback) return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } rclcpp_action::CancelResponse PolyTracker::cancel_callback(const std::shared_ptr goal_handle) { - (void)goal_handle; std::lock_guard lock(mutex_); RCLCPP_INFO(logger_, "PolyTracker goal cancel requested"); - // allow cancel + + // If this is the current active goal, abort it + if(goal_handle == current_goal_handle_ && current_goal_handle_->is_active()) + { + RCLCPP_INFO(logger_, "PolyTracker: aborting current goal due to cancel request"); + goal_set_ = false; + goal_reached_ = true; + active_ = false; + } + + // Allow cancellation return rclcpp_action::CancelResponse::ACCEPT; } void PolyTracker::handle_accepted_callback(const std::shared_ptr goal_handle) { std::lock_guard lock(mutex_); + + // If another goal is already active, abort it before accepting the new one + if(current_goal_handle_ && current_goal_handle_->is_active()) + { + RCLCPP_INFO(logger_, "PolyTracker: aborting previous goal"); + current_goal_handle_->abort(std::make_shared()); + } + // Store the current goal handle so update/activate can reference it current_goal_handle_ = goal_handle; // The goal payload can be accessed via goal_handle->get_goal() From 071c02d4862514a07242d200da178942987f833a Mon Sep 17 00:00:00 2001 From: Dexter Date: Tue, 8 Sep 2026 14:35:40 +0000 Subject: [PATCH 6/6] Working mavros interface --- .../{neurofly_gains.yaml => gains.yaml} | 0 interfaces/kr_mavros_interface/CMakeLists.txt | 125 ++-- .../kr_mavros_interface/config/neurofly.yaml | 17 + ...t.launch.py => mavros_interface.launch.py} | 37 +- .../kr_mavros_interface/launch/test.launch | 29 - .../src/so3cmd_to_mavros_nodelet.cpp | 624 +++++++++++------- .../include/kr_mav_manager/manager.hpp | 15 + kr_mav_manager/src/manager.cpp | 78 ++- .../kr_trackers/src/circle_tracker_server.cpp | 1 + .../src/line_tracker_distance_server.cpp | 2 +- .../src/line_tracker_min_jerk_server.cpp | 2 +- trackers/kr_trackers/src/velocity_tracker.cpp | 1 + 12 files changed, 558 insertions(+), 373 deletions(-) rename interfaces/kr_betaflight_interface/config/{neurofly_gains.yaml => gains.yaml} (100%) create mode 100644 interfaces/kr_mavros_interface/config/neurofly.yaml rename interfaces/kr_mavros_interface/launch/{test.launch.py => mavros_interface.launch.py} (62%) delete mode 100644 interfaces/kr_mavros_interface/launch/test.launch diff --git a/interfaces/kr_betaflight_interface/config/neurofly_gains.yaml b/interfaces/kr_betaflight_interface/config/gains.yaml similarity index 100% rename from interfaces/kr_betaflight_interface/config/neurofly_gains.yaml rename to interfaces/kr_betaflight_interface/config/gains.yaml diff --git a/interfaces/kr_mavros_interface/CMakeLists.txt b/interfaces/kr_mavros_interface/CMakeLists.txt index d538a06c..e24e0ef9 100644 --- a/interfaces/kr_mavros_interface/CMakeLists.txt +++ b/interfaces/kr_mavros_interface/CMakeLists.txt @@ -1,62 +1,63 @@ -cmake_minimum_required(VERSION 3.5) -project(kr_mavros_interface) - -# set default build type -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -endif() - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -add_compile_options(-Wall) - -# ROS 2 configuration -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(kr_mav_msgs REQUIRED) -find_package(mavros_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) - -# If mavros_msgs not found, don't build but do not cause a build error -if(NOT mavros_msgs_FOUND) - message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface") -else() - # Create shared library - add_library(${PROJECT_NAME} SHARED src/so3cmd_to_mavros_nodelet.cpp) - - ament_target_dependencies(${PROJECT_NAME} - rclcpp - rclcpp_components - nav_msgs - geometry_msgs - sensor_msgs - kr_mav_msgs - mavros_msgs - tf2 - tf2_geometry_msgs - Eigen3) - - target_include_directories(${PROJECT_NAME} PUBLIC - $ - $) - - # Register component - rclcpp_components_register_nodes(${PROJECT_NAME} "kr_mavros_interface::SO3CmdToMavros") - - # Install - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - - install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) -endif() - -ament_package() +cmake_minimum_required(VERSION 3.5) +project(kr_mavros_interface) + +# set default build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +add_compile_options(-Wall) + +# ROS 2 configuration +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(kr_mav_msgs REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +# If mavros_msgs not found, don't build but do not cause a build error +if(NOT mavros_msgs_FOUND) + message(WARNING "NOTE: mavros_msgs not found so not building kr_mavros_interface") +else() + # Create shared library + add_library(${PROJECT_NAME} SHARED src/so3cmd_to_mavros_nodelet.cpp) + + ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_components + nav_msgs + geometry_msgs + sensor_msgs + kr_mav_msgs + mavros_msgs + tf2 + tf2_geometry_msgs + Eigen3) + + target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + + # Register component + rclcpp_components_register_nodes(${PROJECT_NAME} "SO3CmdToMavros") + + # Install + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + + install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) + install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) +endif() + +ament_package() diff --git a/interfaces/kr_mavros_interface/config/neurofly.yaml b/interfaces/kr_mavros_interface/config/neurofly.yaml new file mode 100644 index 00000000..8cab5425 --- /dev/null +++ b/interfaces/kr_mavros_interface/config/neurofly.yaml @@ -0,0 +1,17 @@ +/**/so3cmd_to_mavros: + ros__parameters: + + num_props: 4 + thrust_vs_rpm_cof_a: 1.31212977e-08 + thrust_vs_rpm_cof_b: -4.38553e-05 + thrust_vs_rpm_cof_c: 7.89533392e-02 + + lin_cof_a: 5.681818181818182e-05 + lin_int_b: -0.09801136363636374 + + so3_cmd_timeout: 0.25 + odom_timeout: 1.0 + + idle_hold_max_force: 1.0 + + vision_pose_rate: 50.0 diff --git a/interfaces/kr_mavros_interface/launch/test.launch.py b/interfaces/kr_mavros_interface/launch/mavros_interface.launch.py similarity index 62% rename from interfaces/kr_mavros_interface/launch/test.launch.py rename to interfaces/kr_mavros_interface/launch/mavros_interface.launch.py index f35e3af3..a0846dec 100644 --- a/interfaces/kr_mavros_interface/launch/test.launch.py +++ b/interfaces/kr_mavros_interface/launch/mavros_interface.launch.py @@ -1,19 +1,31 @@ +import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode def generate_launch_description(): + # Airframe properties (thrust curve, timeouts, vision_pose rate) live in the + # config file rather than being duplicated as launch arguments here and in + # neurofly_interface/launch/system_launch.launch.py. Override the file with + # config_file:= to fly a different airframe. + default_config_file = os.path.join( + get_package_share_directory("kr_mavros_interface"), + "config", + "neurofly.yaml", + ) + # Declare launch arguments robot_arg = DeclareLaunchArgument("robot", default_value="/", description="Robot namespace") odom_arg = DeclareLaunchArgument("odom", default_value="odom", description="Odometry topic") so3_cmd_arg = DeclareLaunchArgument("so3_cmd", default_value="so3_cmd", description="SO3 command topic") - num_props_arg = DeclareLaunchArgument("num_props", default_value="4", description="Number of propellers") - kf_arg = DeclareLaunchArgument("kf", default_value="2.137145e-6", description="Thrust coefficient") - lin_cof_a_arg = DeclareLaunchArgument("lin_cof_a", default_value="0.0015", description="Linear coefficient A") - lin_int_b_arg = DeclareLaunchArgument("lin_int_b", default_value="-1.5334", description="Linear intercept B") + config_file_arg = DeclareLaunchArgument( + "config_file", default_value=default_config_file, description="SO3CmdToMavros parameter file" + ) # Create composable node so3_cmd_to_mavros_node = ComposableNode( @@ -21,15 +33,7 @@ def generate_launch_description(): plugin="SO3CmdToMavros", name="so3cmd_to_mavros", namespace=LaunchConfiguration("robot"), - parameters=[ - { - "num_props": LaunchConfiguration("num_props"), - "kf": LaunchConfiguration("kf"), - "lin_cof_a": LaunchConfiguration("lin_cof_a"), - "lin_int_b": LaunchConfiguration("lin_int_b"), - "so3_cmd_timeout": 0.25, - } - ], + parameters=[LaunchConfiguration("config_file")], remappings=[ ("~/odom", LaunchConfiguration("odom")), ("~/so3_cmd", LaunchConfiguration("so3_cmd")), @@ -54,10 +58,7 @@ def generate_launch_description(): robot_arg, odom_arg, so3_cmd_arg, - num_props_arg, - kf_arg, - lin_cof_a_arg, - lin_int_b_arg, + config_file_arg, container, ] ) diff --git a/interfaces/kr_mavros_interface/launch/test.launch b/interfaces/kr_mavros_interface/launch/test.launch deleted file mode 100644 index 86f07090..00000000 --- a/interfaces/kr_mavros_interface/launch/test.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp b/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp index f90e4137..b4ecef74 100644 --- a/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp +++ b/interfaces/kr_mavros_interface/src/so3cmd_to_mavros_nodelet.cpp @@ -1,246 +1,378 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -class SO3CmdToMavros : public rclcpp::Node -{ - public: - explicit SO3CmdToMavros(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); - - private: - void so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg); - void odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom); - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose); - - bool odom_set_, imu_set_, so3_cmd_set_; - Eigen::Quaterniond odom_q_, imu_q_; - double kf_, lin_cof_a_, lin_int_b_; - int num_props_; - - rclcpp::Publisher::SharedPtr attitude_raw_pub_; - rclcpp::Publisher::SharedPtr odom_pose_pub_; - - rclcpp::Subscription::SharedPtr so3_cmd_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - - double so3_cmd_timeout_; - rclcpp::Time last_so3_cmd_time_; - kr_mav_msgs::msg::SO3Command last_so3_cmd_; -}; - -void SO3CmdToMavros::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) -{ - odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, - odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); - odom_set_ = true; - - // Publish PoseStamped for mavros vision_pose plugin - auto odom_pose_msg = std::make_shared(); - odom_pose_msg->header = odom->header; - odom_pose_msg->pose = odom->pose.pose; - odom_pose_pub_->publish(*odom_pose_msg); -} - -void SO3CmdToMavros::imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose) -{ - imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); - imu_set_ = true; - - if(so3_cmd_set_ && ((this->now() - last_so3_cmd_time_).seconds() >= so3_cmd_timeout_)) - { - RCLCPP_INFO(this->get_logger(), "so3_cmd timeout. %f seconds since last command", - (this->now() - last_so3_cmd_time_).seconds()); - auto last_so3_cmd_ptr = std::make_shared(last_so3_cmd_); - - so3_cmd_callback(last_so3_cmd_ptr); - } -} - -void SO3CmdToMavros::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) -{ - // both imu_q_ and odom_q_ would be uninitialized if not set - if(!imu_set_) - { - RCLCPP_WARN(this->get_logger(), "Did not receive any imu messages"); - return; - } - - if(!odom_set_) - { - RCLCPP_WARN(this->get_logger(), "Did not receive any odom messages"); - return; - } - - // transform to take into consideration the different yaw of the flight - // controller imu and the odom - // grab desired forces and rotation from so3 - const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z); - - const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); - - // convert to tf2::Quaternion - tf2::Quaternion imu_tf(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf2::Quaternion odom_tf(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); - - // extract RPY's - double imu_roll, imu_pitch, imu_yaw; - double odom_roll, odom_pitch, odom_yaw; - tf2::Matrix3x3(imu_tf).getRPY(imu_roll, imu_pitch, imu_yaw); - tf2::Matrix3x3(odom_tf).getRPY(odom_roll, odom_pitch, odom_yaw); - - // create only yaw tf2::Quaternions - tf2::Quaternion imu_tf_yaw; - tf2::Quaternion odom_tf_yaw; - imu_tf_yaw.setRPY(0.0, 0.0, imu_yaw); - odom_tf_yaw.setRPY(0.0, 0.0, odom_yaw); - const tf2::Quaternion tf_imu_odom_yaw = imu_tf_yaw * odom_tf_yaw.inverse(); - - // transform! - const Eigen::Quaterniond q_des_transformed = - Eigen::Quaterniond(tf_imu_odom_yaw.w(), tf_imu_odom_yaw.x(), tf_imu_odom_yaw.y(), tf_imu_odom_yaw.z()) * q_des; - - // check psi for stability - const Eigen::Matrix3d R_des(q_des); - const Eigen::Matrix3d R_cur(odom_q_); - - const float Psi = 0.5f * (3.0f - (R_des(0, 0) * R_cur(0, 0) + R_des(1, 0) * R_cur(1, 0) + R_des(2, 0) * R_cur(2, 0) + - R_des(0, 1) * R_cur(0, 1) + R_des(1, 1) * R_cur(1, 1) + R_des(2, 1) * R_cur(2, 1) + - R_des(0, 2) * R_cur(0, 2) + R_des(1, 2) * R_cur(1, 2) + R_des(2, 2) * R_cur(2, 2))); - - if(Psi > 1.0f) // Position control stability guaranteed only when Psi < 1 - { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Psi > 1.0, orientation error is too large!"); - } - - double throttle = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); - - // Scale force to individual rotor velocities (rad/s). - throttle = std::sqrt(throttle / num_props_ / kf_); - - // Scaling from rotor velocity (rad/s) to att_throttle for pixhawk - throttle = lin_cof_a_ * throttle + lin_int_b_; - - // failsafe for the error in traj_gen that can lead to nan values - //prevents throttle from being sent to 1 if it is nan. - if (std::isnan(throttle)) - { - throttle = 0.0; - } - - // clamp from 0.0 to 1.0 - throttle = std::min(1.0, throttle); - throttle = std::max(0.0, throttle); - - if(!msg->aux.enable_motors) - throttle = 0; - - // publish messages - auto setpoint_msg = std::make_shared(); - setpoint_msg->header = msg->header; - setpoint_msg->type_mask = 0; - setpoint_msg->orientation.w = q_des_transformed.w(); - setpoint_msg->orientation.x = q_des_transformed.x(); - setpoint_msg->orientation.y = q_des_transformed.y(); - setpoint_msg->orientation.z = q_des_transformed.z(); - setpoint_msg->body_rate.x = msg->angular_velocity.x; - setpoint_msg->body_rate.y = msg->angular_velocity.y; - setpoint_msg->body_rate.z = msg->angular_velocity.z; - setpoint_msg->thrust = throttle; - - attitude_raw_pub_->publish(*setpoint_msg); - - // save last so3_cmd - last_so3_cmd_ = *msg; - last_so3_cmd_time_ = this->now(); - so3_cmd_set_ = true; -} - -SO3CmdToMavros::SO3CmdToMavros(const rclcpp::NodeOptions &options) : rclcpp::Node("so3cmd_to_mavros", options) -{ - // get thrust scaling parameters - if(this->has_parameter("num_props")) - { - this->get_parameter("num_props", num_props_); - RCLCPP_INFO(this->get_logger(), "Got number of props: %d", num_props_); - } - else - { - this->declare_parameter("num_props", 4); - this->get_parameter("num_props", num_props_); - RCLCPP_ERROR(this->get_logger(), "Must set num_props param"); - } - - if(this->has_parameter("kf")) - { - this->get_parameter("kf", kf_); - RCLCPP_INFO(this->get_logger(), "Using kf=%g so that prop speed = sqrt(f / num_props / kf) to scale force to speed.", kf_); - } - else - { - this->declare_parameter("kf", 2.137145e-6); - this->get_parameter("kf", kf_); - RCLCPP_ERROR(this->get_logger(), "Must set kf param for thrust scaling. Motor speed = sqrt(thrust / num_props / kf)"); - } - - if(kf_ <= 0) - { - RCLCPP_FATAL(this->get_logger(), "kf must be positive. kf = %g", kf_); - throw std::invalid_argument("kf must be positive"); - } - - // get thrust scaling parameters - bool has_lin_cof_a = this->has_parameter("lin_cof_a"); - bool has_lin_int_b = this->has_parameter("lin_int_b"); - - if(has_lin_cof_a && has_lin_int_b) - { - this->get_parameter("lin_cof_a", lin_cof_a_); - this->get_parameter("lin_int_b", lin_int_b_); - RCLCPP_INFO(this->get_logger(), "Using %g*x + %g to scale prop speed to att_throttle.", lin_cof_a_, lin_int_b_); - } - else - { - this->declare_parameter("lin_cof_a", 0.0015); - this->declare_parameter("lin_int_b", -1.5334); - this->get_parameter("lin_cof_a", lin_cof_a_); - this->get_parameter("lin_int_b", lin_int_b_); - RCLCPP_ERROR(this->get_logger(), - "Must set coefficients for thrust scaling (scaling from rotor velocity (rad/s) to att_throttle for pixhawk)"); - } - - // get param for so3 command timeout duration - this->declare_parameter("so3_cmd_timeout", 0.25); - this->get_parameter("so3_cmd_timeout", so3_cmd_timeout_); - - odom_set_ = false; - imu_set_ = false; - so3_cmd_set_ = false; - last_so3_cmd_time_ = this->now(); - - attitude_raw_pub_ = this->create_publisher("~/attitude_raw", 10); - odom_pose_pub_ = this->create_publisher("~/odom_pose", 10); - - so3_cmd_sub_ = this->create_subscription( - "~/so3_cmd", 10, std::bind(&SO3CmdToMavros::so3_cmd_callback, this, std::placeholders::_1)); - - odom_sub_ = this->create_subscription( - "~/odom", 10, std::bind(&SO3CmdToMavros::odom_callback, this, std::placeholders::_1)); - - imu_sub_ = this->create_subscription( - "~/imu", 10, std::bind(&SO3CmdToMavros::imu_callback, this, std::placeholders::_1)); - - RCLCPP_INFO(this->get_logger(), "SO3CmdToMavros node initialized"); -} - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToMavros) +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +static constexpr double kYawOffsetWarnThreshold = 5.0 * M_PI / 180.0; + +static std::pair solve_quadratic(double a, double b, double c) +{ + const double term1 = -b, term2 = std::sqrt(b * b - 4 * a * c); + return std::make_pair((term1 + term2) / (2 * a), (term1 - term2) / (2 * a)); +} + +class SO3CmdToMavros : public rclcpp::Node +{ + public: + explicit SO3CmdToMavros(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + private: + void so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom); + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose); + void publish_attitude_target(const kr_mav_msgs::msg::SO3Command &msg, bool fresh_command); + + bool odom_set_, imu_set_, so3_cmd_set_; + Eigen::Quaterniond odom_q_, imu_q_; + double thrust_vs_rpm_cof_a_, thrust_vs_rpm_cof_b_, thrust_vs_rpm_cof_c_; + double lin_cof_a_, lin_int_b_; + int num_props_; + + rclcpp::Publisher::SharedPtr attitude_raw_pub_; + rclcpp::Publisher::SharedPtr odom_pose_pub_; + + rclcpp::Subscription::SharedPtr so3_cmd_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + + double so3_cmd_timeout_; + double odom_timeout_; + double idle_hold_max_force_; + double vision_pose_period_; + rclcpp::Time next_vision_pose_time_; + rclcpp::Time last_so3_cmd_time_; + rclcpp::Time last_odom_time_; + kr_mav_msgs::msg::SO3Command last_so3_cmd_; +}; + +void SO3CmdToMavros::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) +{ + const auto &p = odom->pose.pose.position; + const auto &q = odom->pose.pose.orientation; + + // Reject invalid odom rather than feeding it to the controller and to EKF2 via + // vision_pose. Matches the guard the betaflight interface already has. + if(!std::isfinite(q.w) || !std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || + !std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z)) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Received non-finite odom pose, ignoring"); + odom_set_ = false; + return; + } + + const rclcpp::Time now = this->now(); + + odom_q_ = Eigen::Quaterniond(q.w, q.x, q.y, q.z); + odom_set_ = true; + last_odom_time_ = now; + + // Deadline accumulator rather than a simple "elapsed >= period" test. The latter + // aliases: at 114 Hz odom against a 20 ms budget, the sample at 17.5 ms is + // rejected and the next lands at 26.3 ms, so it publishes every 3rd sample and + // yields 38 Hz, not 50. Advancing the deadline by exactly one period keeps the + // long-run average on target (alternating 2 and 3 input samples). + if(vision_pose_period_ > 0.0) + { + if(now < next_vision_pose_time_) + return; + next_vision_pose_time_ = next_vision_pose_time_ + rclcpp::Duration::from_seconds(vision_pose_period_); + // Resync if we fell behind (odom stalled, clock jump) instead of bursting to + // catch up on a backlog that no longer matters. + if(next_vision_pose_time_ < now) + next_vision_pose_time_ = now + rclcpp::Duration::from_seconds(vision_pose_period_); + } + + auto odom_pose_msg = std::make_shared(); + odom_pose_msg->header = odom->header; + odom_pose_msg->pose = odom->pose.pose; + odom_pose_pub_->publish(*odom_pose_msg); +} + +void SO3CmdToMavros::imu_callback(const sensor_msgs::msg::Imu::SharedPtr pose) +{ + imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); + imu_set_ = true; + + if(!so3_cmd_set_) + return; + + const rclcpp::Time now = this->now(); + const double cmd_age = (now - last_so3_cmd_time_).seconds(); + if(cmd_age < so3_cmd_timeout_) + return; // commands are flowing normally; nothing to do + + const double odom_age = (now - last_odom_time_).seconds(); + if(!odom_set_ || odom_age > odom_timeout_) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "so3_cmd stale (%.2f s) AND odom stale (%.2f s): not sending setpoints. " + "PX4 offboard failsafe should engage.", + cmd_age, odom_age); + return; + } + + const double f = std::sqrt(last_so3_cmd_.force.x * last_so3_cmd_.force.x + + last_so3_cmd_.force.y * last_so3_cmd_.force.y + + last_so3_cmd_.force.z * last_so3_cmd_.force.z); + if(f > idle_hold_max_force_) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "so3_cmd stale (%.2f s) while airborne (last commanded force %.2f N): NOT holding. " + "PX4 offboard failsafe should engage.", + cmd_age, f); + return; + } + + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "so3_cmd idle for %.2f s (odom healthy, force %.3f N): holding level at current yaw " + "to keep OFFBOARD alive.", + cmd_age, f); + + kr_mav_msgs::msg::SO3Command hold = last_so3_cmd_; + const double odom_yaw = std::atan2(2.0 * (odom_q_.w() * odom_q_.z() + odom_q_.x() * odom_q_.y()), + 1.0 - 2.0 * (odom_q_.y() * odom_q_.y() + odom_q_.z() * odom_q_.z())); + hold.orientation.w = std::cos(odom_yaw / 2.0); + hold.orientation.x = 0.0; + hold.orientation.y = 0.0; + hold.orientation.z = std::sin(odom_yaw / 2.0); + + publish_attitude_target(hold, false); +} + +void SO3CmdToMavros::so3_cmd_callback(const kr_mav_msgs::msg::SO3Command::SharedPtr msg) +{ + // Record the genuine command first, so the idle-hold path in imu_callback has + // something to re-send and can measure how long it has been since a real one. + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = this->now(); + so3_cmd_set_ = true; + + publish_attitude_target(*msg, true); +} + +void SO3CmdToMavros::publish_attitude_target(const kr_mav_msgs::msg::SO3Command &cmd, bool fresh_command) +{ + const auto msg = &cmd; + + // both imu_q_ and odom_q_ would be uninitialized if not set. Throttled because + // mav_services queues a burst of commands from set_motors(), which on a normal + // startup arrives before mavros has finished connecting to the FCU. + if(!imu_set_) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Did not receive any imu messages (is mavros connected to the FCU?)"); + return; + } + + if(!odom_set_) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Did not receive any odom messages"); + return; + } + + // transform to take into consideration the different yaw of the flight + // controller imu and the odom + // grab desired forces and rotation from so3 + const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z); + + const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); + + // convert to tf2::Quaternion + tf2::Quaternion imu_tf(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); + tf2::Quaternion odom_tf(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + + // extract RPY's + double imu_roll, imu_pitch, imu_yaw; + double odom_roll, odom_pitch, odom_yaw; + tf2::Matrix3x3(imu_tf).getRPY(imu_roll, imu_pitch, imu_yaw); + tf2::Matrix3x3(odom_tf).getRPY(odom_roll, odom_pitch, odom_yaw); + + // create only yaw tf2::Quaternions + tf2::Quaternion imu_tf_yaw; + tf2::Quaternion odom_tf_yaw; + imu_tf_yaw.setRPY(0.0, 0.0, imu_yaw); + odom_tf_yaw.setRPY(0.0, 0.0, odom_yaw); + const tf2::Quaternion tf_imu_odom_yaw = imu_tf_yaw * odom_tf_yaw.inverse(); + + // Diagnostic: how far the FCU's heading estimate has drifted from the odom heading. + // This correction silently rotates the commanded tilt direction, so a large offset + // means the position loop is fighting a rotated frame. + const double yaw_offset = std::atan2(std::sin(imu_yaw - odom_yaw), std::cos(imu_yaw - odom_yaw)); + if(std::abs(yaw_offset) > kYawOffsetWarnThreshold) + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "FCU yaw and odom yaw differ by %.1f deg. Check that EKF2 is fusing the " + "vision pose for yaw.", + yaw_offset * 180.0 / M_PI); + } + + // transform! + const Eigen::Quaterniond q_des_transformed = + Eigen::Quaterniond(tf_imu_odom_yaw.w(), tf_imu_odom_yaw.x(), tf_imu_odom_yaw.y(), tf_imu_odom_yaw.z()) * q_des; + + // check psi for stability + const Eigen::Matrix3d R_des(q_des); + const Eigen::Matrix3d R_cur(odom_q_); + + const float Psi = 0.5f * (3.0f - (R_des(0, 0) * R_cur(0, 0) + R_des(1, 0) * R_cur(1, 0) + R_des(2, 0) * R_cur(2, 0) + + R_des(0, 1) * R_cur(0, 1) + R_des(1, 1) * R_cur(1, 1) + R_des(2, 1) * R_cur(2, 1) + + R_des(0, 2) * R_cur(0, 2) + R_des(1, 2) * R_cur(1, 2) + R_des(2, 2) * R_cur(2, 2))); + + if(fresh_command && Psi > 1.0f) // Position control stability guaranteed only when Psi < 1 + { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Psi > 1.0, orientation error is too large!"); + } + + double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); + + // Scale thrust to individual rotor velocities (RPM) via quadratic thrust curve. + const double avg_thrust = std::max(0.0, thrust) / num_props_; + const auto rpm_solutions = + solve_quadratic(thrust_vs_rpm_cof_a_, thrust_vs_rpm_cof_b_, thrust_vs_rpm_cof_c_ - avg_thrust); + const double omega_avg = std::max(rpm_solutions.first, rpm_solutions.second); + + // Scaling from rotor velocity (RPM) to att_throttle for pixhawk + double throttle = lin_cof_a_ * omega_avg + lin_int_b_; + + // failsafe for the error in traj_gen that can lead to nan values + //prevents throttle from being sent to 1 if it is nan. + if (std::isnan(throttle)) + { + throttle = 0.0; + } + + // clamp from 0.0 to 1.0 + throttle = std::min(1.0, throttle); + throttle = std::max(0.0, throttle); + + if(!msg->aux.enable_motors) + throttle = 0; + + // publish messages + auto setpoint_msg = std::make_shared(); + setpoint_msg->header = msg->header; + // Stamp with now(), not the original command's stamp: on the idle-hold path the + // stored command can be seconds old and PX4 should see a current setpoint. + setpoint_msg->header.stamp = this->now(); + setpoint_msg->type_mask = 0; + setpoint_msg->orientation.w = q_des_transformed.w(); + setpoint_msg->orientation.x = q_des_transformed.x(); + setpoint_msg->orientation.y = q_des_transformed.y(); + setpoint_msg->orientation.z = q_des_transformed.z(); + setpoint_msg->body_rate.x = msg->angular_velocity.x; + setpoint_msg->body_rate.y = msg->angular_velocity.y; + setpoint_msg->body_rate.z = msg->angular_velocity.z; + setpoint_msg->thrust = throttle; + + attitude_raw_pub_->publish(*setpoint_msg); +} + +SO3CmdToMavros::SO3CmdToMavros(const rclcpp::NodeOptions &options) : rclcpp::Node("so3cmd_to_mavros", options) +{ + const auto &overrides = this->get_node_parameters_interface()->get_parameter_overrides(); + auto was_configured = [&overrides](const std::string &name) { return overrides.count(name) > 0; }; + + this->declare_parameter("num_props", 4); + this->get_parameter("num_props", num_props_); + if(was_configured("num_props")) + RCLCPP_INFO(this->get_logger(), "Got number of props: %d", num_props_); + else + RCLCPP_ERROR(this->get_logger(), "num_props not set, defaulting to %d", num_props_); + + // quadratic thrust-vs-RPM curve: thrust_per_prop = a*omega^2 + b*omega + c + this->declare_parameter("thrust_vs_rpm_cof_a", 2.137145e-6); + this->declare_parameter("thrust_vs_rpm_cof_b", 0.0); + this->declare_parameter("thrust_vs_rpm_cof_c", 0.0); + this->get_parameter("thrust_vs_rpm_cof_a", thrust_vs_rpm_cof_a_); + this->get_parameter("thrust_vs_rpm_cof_b", thrust_vs_rpm_cof_b_); + this->get_parameter("thrust_vs_rpm_cof_c", thrust_vs_rpm_cof_c_); + + if(was_configured("thrust_vs_rpm_cof_a") && was_configured("thrust_vs_rpm_cof_b") && + was_configured("thrust_vs_rpm_cof_c")) + { + RCLCPP_INFO(this->get_logger(), "Using thrust = %g*omega^2 + %g*omega + %g to scale force to rotor speed.", + thrust_vs_rpm_cof_a_, thrust_vs_rpm_cof_b_, thrust_vs_rpm_cof_c_); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Must set thrust_vs_rpm_cof_{a,b,c} params for thrust scaling."); + } + + if(thrust_vs_rpm_cof_a_ <= 0) + { + RCLCPP_FATAL(this->get_logger(), "thrust_vs_rpm_cof_a must be positive. thrust_vs_rpm_cof_a = %g", thrust_vs_rpm_cof_a_); + throw std::invalid_argument("thrust_vs_rpm_cof_a must be positive"); + } + + // scaling from rotor velocity (RPM) to att_throttle for pixhawk + this->declare_parameter("lin_cof_a", 0.0015); + this->declare_parameter("lin_int_b", -1.5334); + this->get_parameter("lin_cof_a", lin_cof_a_); + this->get_parameter("lin_int_b", lin_int_b_); + + if(was_configured("lin_cof_a") && was_configured("lin_int_b")) + { + RCLCPP_INFO(this->get_logger(), "Using %g*x + %g to scale prop speed to att_throttle.", lin_cof_a_, lin_int_b_); + } + else + { + RCLCPP_ERROR(this->get_logger(), + "Must set coefficients for thrust scaling (scaling from rotor velocity (RPM) to att_throttle for pixhawk)"); + } + + // get param for so3 command timeout duration + this->declare_parameter("so3_cmd_timeout", 0.25); + this->get_parameter("so3_cmd_timeout", so3_cmd_timeout_); + + // How stale odom may be before the idle-hold in imu_callback stops re-sending + // the last command and lets PX4's offboard-loss failsafe take over. + this->declare_parameter("odom_timeout", 0.2); + this->get_parameter("odom_timeout", odom_timeout_); + + // Above this commanded force the vehicle is considered airborne and the + // idle-hold refuses to engage. Ground idle commands ~0 N (set_motors sends + // FLT_MIN); hover for this airframe is ~7.6 N, so 1 N separates them cleanly. + this->declare_parameter("idle_hold_max_force", 1.0); + this->get_parameter("idle_hold_max_force", idle_hold_max_force_); + + // Max rate for the vision_pose stream to the FCU, in Hz. 0 = unthrottled + // (the old behaviour, which overflowed the serial TX queue at ~114 Hz). + this->declare_parameter("vision_pose_rate", 50.0); + double vision_pose_rate = this->get_parameter("vision_pose_rate").as_double(); + vision_pose_period_ = (vision_pose_rate > 0.0) ? (1.0 / vision_pose_rate) : 0.0; + RCLCPP_INFO(this->get_logger(), "vision_pose to FCU limited to %.1f Hz", vision_pose_rate); + + odom_set_ = false; + imu_set_ = false; + so3_cmd_set_ = false; + last_so3_cmd_time_ = this->now(); + last_odom_time_ = this->now(); + next_vision_pose_time_ = this->now(); + + attitude_raw_pub_ = this->create_publisher("~/attitude_raw", 10); + odom_pose_pub_ = this->create_publisher("~/odom_pose", 10); + + so3_cmd_sub_ = this->create_subscription( + "~/so3_cmd", 10, std::bind(&SO3CmdToMavros::so3_cmd_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "~/odom", rclcpp::SensorDataQoS(), std::bind(&SO3CmdToMavros::odom_callback, this, std::placeholders::_1)); + + imu_sub_ = this->create_subscription( + "~/imu", rclcpp::SensorDataQoS(), std::bind(&SO3CmdToMavros::imu_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "SO3CmdToMavros node initialized"); +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(SO3CmdToMavros) diff --git a/kr_mav_manager/include/kr_mav_manager/manager.hpp b/kr_mav_manager/include/kr_mav_manager/manager.hpp index 35f3b487..c1a7c17c 100644 --- a/kr_mav_manager/include/kr_mav_manager/manager.hpp +++ b/kr_mav_manager/include/kr_mav_manager/manager.hpp @@ -3,6 +3,7 @@ // Standard C++ Libraries #include #include +#include #include // ROS2 related @@ -175,6 +176,12 @@ class MAVManager : public rclcpp::Node rclcpp::Time last_odom_t_, last_imu_t_, last_output_data_t_, last_heartbeat_t_; + // Guards the odom-derived state below. odometry_cb runs in odom_cb_group_ while + // heartbeat() runs in watchdog_cb_group_, so these are genuinely concurrent. + // Only ever held for plain field copies -- never across a blocking service or + // action call, which would deadlock the watchdog against the odom stream. + mutable std::mutex odom_state_mutex_; + Vec3 pos_, vel_; float mass_; Quat odom_q_, imu_q_; @@ -210,6 +217,14 @@ class MAVManager : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_goal_velocity_; // pub_goal_yaw_ and pub_pwm_command_ defined in ros1 package but not being used + // Drives heartbeat() from the wall clock so the odom/imu watchdogs are not + // clocked by the very data they are checking for. Both of these live in their + // own callback groups so a blocking service/action callback cannot starve the + // odom path, and so eland() blocking inside the watchdog cannot starve it either. + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::CallbackGroup::SharedPtr odom_cb_group_; + rclcpp::CallbackGroup::SharedPtr watchdog_cb_group_; + // Subscribers rclcpp::Subscription::SharedPtr odom_sub_; // odometry_cb rclcpp::Subscription::SharedPtr heartbeat_sub_; // heartbeat_cb diff --git a/kr_mav_manager/src/manager.cpp b/kr_mav_manager/src/manager.cpp index 667e9ccc..d92026e7 100644 --- a/kr_mav_manager/src/manager.cpp +++ b/kr_mav_manager/src/manager.cpp @@ -116,7 +116,7 @@ MAVManager::MAVManager() // Publishers pub_motors_ = this->create_publisher("motors", 10); pub_estop_ = this->create_publisher("estop", 10); - pub_so3_command_ = this->create_publisher("so3_controller/so3_cmd", 10); + pub_so3_command_ = this->create_publisher("so3_cmd", 10); pub_trpy_command_ = this->create_publisher("trpy_cmd", 10); pub_position_command_ = this->create_publisher("trackers_manager/cmd", 10); pub_status_ = this->create_publisher("~/status", 10); @@ -127,9 +127,11 @@ MAVManager::MAVManager() rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - // Subscribers - odom_sub_ = - this->create_subscription("control_odom", qos, std::bind(&MAVManager::odometry_cb, this, _1)); + odom_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions odom_options; + odom_options.callback_group = odom_cb_group_; + odom_sub_ = this->create_subscription( + "control_odom", qos, std::bind(&MAVManager::odometry_cb, this, _1), odom_options); heartbeat_sub_ = this->create_subscription("heartbeat", qos, std::bind(&MAVManager::heartbeat_cb, this, _1)); tracker_status_sub_ = this->create_subscription( @@ -167,11 +169,19 @@ MAVManager::MAVManager() "quad_decode_msg/output_data", 10, std::bind(&MAVManager::output_data_cb, this, _1)); } - if(!this->get_parameter("use_attitide_safety_catch", use_attitude_safety_catch_)) + if(!this->get_parameter("use_attitude_safety_catch", use_attitude_safety_catch_)) { RCLCPP_WARN(this->get_logger(), "Couldn't find use_attitude_safety_catch param"); } + double max_att_angle; + if(this->get_parameter("max_attitude_angle", max_att_angle)) + { + max_attitude_angle_ = static_cast(max_att_angle); + } + RCLCPP_INFO(this->get_logger(), "Attitude safety catch %s, max attitude angle %2.1f deg", + use_attitude_safety_catch_ ? "enabled" : "disabled", max_attitude_angle_ * 180.0 / M_PI); + double m; if(!this->get_parameter("mass", m)) { @@ -193,6 +203,9 @@ MAVManager::MAVManager() RCLCPP_ERROR(this->get_logger(), "Could not disable motors"); } construction_done = true; + watchdog_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + heartbeat_timer_ = + this->create_wall_timer(std::chrono::milliseconds(50), [this]() { this->heartbeat(); }, watchdog_cb_group_); } void MAVManager::tracker_done_callback(const LineTrackerGoalHandle::WrappedResult &result) @@ -337,6 +350,8 @@ bool MAVManager::sendPolyGoal(const PolyTracker::Goal &goal_msg) void MAVManager::odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg) { + std::lock_guard lock(odom_state_mutex_); + pos_(0) = msg->pose.pose.position.x; pos_(1) = msg->pose.pose.position.y; pos_(2) = msg->pose.pose.position.z; @@ -353,8 +368,6 @@ void MAVManager::odometry_cb(nav_msgs::msg::Odometry::ConstSharedPtr msg) yaw_dot_ = msg->twist.twist.angular.z; last_odom_t_ = this->now(); - - this->heartbeat(); } bool MAVManager::takeoff() @@ -945,13 +958,19 @@ void MAVManager::heartbeat() // Only need to do monitoring at the specified frequency rclcpp::Time t = this->now(); - if (!last_heartbeat_t_initialized_) { + if(!last_heartbeat_t_initialized_) + { last_heartbeat_t_ = t; + last_heartbeat_t_initialized_ = true; return; } + // 10% tolerance: heartbeat_timer_ fires every 50 ms against a 100 ms budget, so + // an exact comparison aliases -- a tick landing at 99 ms is skipped and the next + // lands at ~150 ms, yielding ~8 Hz with 150 ms gaps instead of a steady 10 Hz. + // The gap sets the watchdog's detection granularity, so keep it tight. float dt = (t - last_heartbeat_t_).seconds(); - if(dt < 1 / freq) + if(dt < 0.9f / freq) return; else last_heartbeat_t_ = t; @@ -961,17 +980,18 @@ void MAVManager::heartbeat() status_msg.data = status_; pub_status_->publish(status_msg); - // Checking for odom + // Checking for odom. Throttled -- this re-evaluates at 10 Hz and the fault + // usually persists for many cycles. if(this->motors() && need_odom_ && !this->have_recent_odom()) { - RCLCPP_WARN(this->get_logger(), "No recent odometry!"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No recent odometry!"); this->eland(); } // Checking for imu if(this->motors() && need_imu_ && !this->have_recent_imu()) { - RCLCPP_WARN(this->get_logger(), "No recent imu!"); + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No recent imu!"); this->eland(); } @@ -981,9 +1001,18 @@ void MAVManager::heartbeat() // position commands. Maybe put a timeout, but it could be dangerous? Maybe // require a call to hover before exiting a safety catch mode? + // Snapshot the odom-derived attitude under the lock; a torn read here would + // produce a garbage geodesic and could trigger a spurious ehover. + Quat imu_q_snapshot, odom_q_snapshot; + { + std::lock_guard lock(odom_state_mutex_); + imu_q_snapshot = imu_q_; + odom_q_snapshot = odom_q_; + } + // Convert quaternions to tf so we can compute Euler angles, etc - tf2::Quaternion imu_q(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); - tf2::Quaternion odom_q(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + tf2::Quaternion imu_q(imu_q_snapshot.x(), imu_q_snapshot.y(), imu_q_snapshot.z(), imu_q_snapshot.w()); + tf2::Quaternion odom_q(odom_q_snapshot.x(), odom_q_snapshot.y(), odom_q_snapshot.z(), odom_q_snapshot.w()); // determine a geodesic angle from hover at the same yaw double yaw, pitch, roll; @@ -1056,11 +1085,27 @@ bool MAVManager::eland() // left the ground, we don't want them to spin up faster. if(this->motors() && (status_ == FLYING || status_ == ELAND)) { - RCLCPP_WARN(this->get_logger(), "Emergency Land"); + // Throttled: heartbeat() re-enters this at 10 Hz for as long as the fault + // persists, and an unthrottled WARN here buries everything else in the log. + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Emergency Land"); auto goal = kr_mav_msgs::msg::PositionCommand(); goal.acceleration.z = -0.45f; - goal.yaw = yaw_; + { + std::lock_guard lock(odom_state_mutex_); + // Hold the CURRENT horizontal position. These were previously left at their + // default of 0, and SO3ControlComponent uses cmd->position directly as + // des_pos_ (there is no ignore-position flag in PositionCommand). So an eland + // while hovering away from the origin commanded a flight *to* the origin: in + // a recorded flight the vehicle was hovering at y=9.0 m, elanded, and was told + // to go to y=0 -- it saturated at the tilt limit and flew 5.5 m across the + // room before the pilot took over. + goal.position.x = pos_(0); + goal.position.y = pos_(1); + goal.yaw = yaw_; + } + // position.z is deliberately left at 0: the resulting negative z error is what + // drives the descent, together with acceleration.z above. Unchanged behaviour. if(this->setPositionCommand(goal)) { @@ -1206,6 +1251,7 @@ bool MAVManager::send_transition_request(std::shared_ptr lock(odom_state_mutex_); return (this->now() - last_odom_t_).seconds() < odom_timeout_; } diff --git a/trackers/kr_trackers/src/circle_tracker_server.cpp b/trackers/kr_trackers/src/circle_tracker_server.cpp index 5ee5a594..f66bcd03 100644 --- a/trackers/kr_trackers/src/circle_tracker_server.cpp +++ b/trackers/kr_trackers/src/circle_tracker_server.cpp @@ -13,6 +13,7 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "std_msgs/msg/empty.hpp" #include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include diff --git a/trackers/kr_trackers/src/line_tracker_distance_server.cpp b/trackers/kr_trackers/src/line_tracker_distance_server.cpp index f5502732..981f7762 100644 --- a/trackers/kr_trackers/src/line_tracker_distance_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_distance_server.cpp @@ -56,7 +56,7 @@ class LineTrackerDistance : public kr_trackers_manager::Tracker // Distance traveled to get to last goal. float current_traj_length_; - std::shared_ptr result_; + std::shared_ptr result_ = std::make_shared(); }; LineTrackerDistance::LineTrackerDistance() diff --git a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp index 55623fe2..9a58470e 100644 --- a/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp +++ b/trackers/kr_trackers/src/line_tracker_min_jerk_server.cpp @@ -64,7 +64,7 @@ class LineTrackerMinJerk : public kr_trackers_manager::Tracker float goal_yaw_, yaw_coeffs_[4]; float current_traj_length_; - std::shared_ptr result_; + std::shared_ptr result_ = std::make_shared(); }; LineTrackerMinJerk::LineTrackerMinJerk(void) diff --git a/trackers/kr_trackers/src/velocity_tracker.cpp b/trackers/kr_trackers/src/velocity_tracker.cpp index 7048c1ab..0ddcedc3 100644 --- a/trackers/kr_trackers/src/velocity_tracker.cpp +++ b/trackers/kr_trackers/src/velocity_tracker.cpp @@ -8,6 +8,7 @@ #include "kr_tracker_msgs/msg/tracker_status.hpp" #include "kr_trackers/Tracker.hpp" #include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include class VelocityTracker : public kr_trackers_manager::Tracker