diff --git a/.env b/.env index 65b287be7..0e0729f7a 100644 --- a/.env +++ b/.env @@ -1,7 +1,7 @@ # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml PROJECT_NAME="airstack" # auto-generated from git commit hash -DOCKER_IMAGE_TAG="7c21230" +DOCKER_IMAGE_TAG="f3f93a5" # can replace with your docker hub username PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared" DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/AFCA/fire_academy_faro_with_sky.scene.usd" diff --git a/common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp b/common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp index a1131a924..33d68aa3a 100644 --- a/common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp +++ b/common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp @@ -84,6 +84,59 @@ namespace airstack { return get_param(node.get(), name, default_value, &set); } + + struct ParamInfo{ + void* variable; + rclcpp::ParameterType type; + }; + std::unordered_map dynamic_params; + rclcpp::Node* temp_node = NULL; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle; + + rcl_interfaces::msg::SetParametersResult on_param_change(const std::vector & params){ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + if(temp_node != NULL){ + for (const auto & param : params) { + RCLCPP_INFO_STREAM(temp_node->get_logger(), param.get_name()); + + if(dynamic_params.count(param.get_name()) > 0){ + ParamInfo pi = dynamic_params[param.get_name()]; + + if(pi.type == rclcpp::ParameterType::PARAMETER_INTEGER) + *((int*)pi.variable) = param.as_int(); + else if(pi.type == rclcpp::ParameterType::PARAMETER_DOUBLE) + *((double*)pi.variable) = param.as_double(); + else if(pi.type == rclcpp::ParameterType::PARAMETER_STRING) + *((std::string*)pi.variable) = param.as_string(); + else if(pi.type == rclcpp::ParameterType::PARAMETER_BOOL) + *((bool*)pi.variable) = param.as_bool(); + } + } + } + + + + return result; + } + + template + inline void dynamic_param(rclcpp::Node* node, std::string name, T default_value, T* variable){ + temp_node = node; + //rclcpp::ParameterValue pv = node->declare_parameter(name, rclcpp::ParameterValue(default_value)); + //*variable = default_value; + *variable = get_param(node, name, default_value); + + ParamInfo pi; + pi.variable = (void*)variable; + pi.type = node->get_parameter(name).get_type();//pv.get_type(); + dynamic_params[name] = pi; + + if(!param_callback_handle) + param_callback_handle = node->add_on_set_parameters_callback(&on_param_change); + } + // services template diff --git a/common/ros_packages/straps_msgs b/common/ros_packages/straps_msgs deleted file mode 160000 index 290987632..000000000 --- a/common/ros_packages/straps_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 290987632f7486bca099f5552f49f4f9dd100112 diff --git a/ground_control_station/docker/Dockerfile.gcs b/ground_control_station/docker/Dockerfile.gcs index 12090ca4a..877fc0362 100644 --- a/ground_control_station/docker/Dockerfile.gcs +++ b/ground_control_station/docker/Dockerfile.gcs @@ -18,11 +18,12 @@ RUN apt-get update && apt-get install -y \ gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-tools \ gstreamer1.0-x gstreamer1.0-alsa openssh-server \ xterm gnome-terminal libcanberra-gtk-module libcanberra-gtk3-module dbus-x11 \ + ros-humble-rosbag2-storage-mcap ros-humble-plotjuggler ros-humble-plotjuggler-ros \ && rm -rf /var/lib/apt/lists/* # Install Python dependencies RUN pip3 install empy future lxml matplotlib numpy pkgconfig psutil pygments \ - wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt sphinx + wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt sphinx utm # Configure SSH RUN mkdir /var/run/sshd && echo 'root:airstack' | chpasswd && \ diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/control.xml b/ground_control_station/ros_ws/src/gcs_bringup/config/control.xml new file mode 100644 index 000000000..7a1259f46 --- /dev/null +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/control.xml @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective index 47027615e..548cfebe2 100644 --- a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective @@ -4,14 +4,14 @@ "mainwindow": { "keys": { "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000014b00000069000008710000045c0000014b0000008e000008710000045c00000000000000000a000000014b0000008e000008710000045c')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000001df00000063000009ad000004a2000001df00000088000009ad000004a200000000000000000a00000001df00000088000009ad000004a2')", "type": "repr(QByteArray.hex)", - "pretty-print": " K i q \\ K q \\ K q \\" + "pretty-print": " c " }, "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000727000003a5fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f01000000000000039c000002cd00fffffffc000003a200000385000000d000fffffffa000000010200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000ffffffff0000004a00fffffffb0000006a007200710074005f0061006900720073007400610063006b005f0063006f006e00740072006f006c005f00700061006e0065006c005f005f0041006900720073007400610063006b0043006f006e00740072006f006c00500061006e0065006c005f005f0031005f005f0100000000ffffffff0000036300ffffff000007270000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000007cf000003f1fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f0100000000000003e9000002cd00fffffffc000003ef000003e0000000d000fffffffa000000010200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000ffffffff0000004a00fffffffb0000006a007200710074005f0061006900720073007400610063006b005f0063006f006e00740072006f006c005f00700061006e0065006c005f005f0041006900720073007400610063006b0043006f006e00740072006f006c00500061006e0065006c005f005f0031005f005f0100000000ffffffff0000036300ffffff000007cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", "type": "repr(QByteArray.hex)", - "pretty-print": " jrqt_ground_control_station__GroundControlStation__1__ J c ' " + "pretty-print": " jrqt_ground_control_station__GroundControlStation__1__ J c " } }, "groups": { @@ -57,7 +57,7 @@ "plugin": { "keys": { "info_dcts": { - "repr": "[{'enable_display': True, 'excluded_services': [], 'hostname': '172.17.0.1', 'name': 'Localhost', 'namespace': 'none', 'password': '\\U000f01de\\U000f01db\\U000f01dc\\U000f01da', 'path': '~/airstack', 'username': 'john'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.11', 'name': 'NX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.12', 'name': 'AGX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.21', 'name': 'NX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.22', 'name': 'AGX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.31', 'name': 'NX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.32', 'name': 'AGX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}]", + "repr": "[{'enable_display': True, 'excluded_services': [], 'hostname': '172.17.0.1', 'name': 'Localhost', 'namespace': 'none', 'password': '\\U000f01de\\U000f01db\\U000f01dc\\U000f01da', 'path': '~/temp_airstack/merge/airstack', 'username': 'john'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.11', 'name': 'NX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.12', 'name': 'AGX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.21', 'name': 'NX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.22', 'name': 'AGX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.31', 'name': 'NX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.32', 'name': 'AGX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}]", "type": "repr" } }, diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml index 1554948ba..dc3a6513f 100644 --- a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml @@ -3,12 +3,13 @@ + the base_link tf --> + + - + @@ -42,27 +44,37 @@ + + + + + + + + odometry_conversion now isn't creating this from odometry estimate --> + #include #include +#include #include #include "GeographicLib/Geoid.hpp" @@ -321,6 +322,7 @@ class MAVROSInterface : public robot_interface::RobotInterface { rclcpp::Publisher::SharedPtr global_position_target_pub_; rclcpp::Publisher::SharedPtr velocity_target_pub_; rclcpp::Publisher::SharedPtr set_home_pub_; + rclcpp::Publisher::SharedPtr reset_integrators_pub_; rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr home_sub_; @@ -380,6 +382,9 @@ class MAVROSInterface : public robot_interface::RobotInterface { set_home_pub_ = this->create_publisher( "mavros/home_position/set", qos_profile); + reset_integrators_pub_ = this->create_publisher( + "reset_integrators", 1); + // subscribers state_sub_ = this->create_subscription( "mavros/state", 1, @@ -467,30 +472,40 @@ class MAVROSInterface : public robot_interface::RobotInterface { void roll_pitch_yawrate_thrust_callback( const mav_msgs::msg::RollPitchYawrateThrust::SharedPtr cmd) override { + /* if (!is_yaw_received_) { RCLCPP_ERROR(this->get_logger(), "roll_pitch_yawrate_thrust command called but haven't yet " "received drone current yaw"); return; } + */ + if (!is_ardupilot || + (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > post_takeoff_command_delay_time))) { + + if(fabs((this->get_clock()->now() - in_air_start_time).seconds() - post_takeoff_command_delay_time) < 1.){ + std_msgs::msg::Empty empty; + reset_integrators_pub_->publish(empty); + } + + mavros_msgs::msg::AttitudeTarget mavros_cmd; + mavros_cmd.header.stamp = this->get_clock()->now(); //.to_msg(); + mavros_cmd.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_YAW_RATE; + //mavros_cmd.body_rate.z = cmd->yaw_rate; + mavros_cmd.thrust = cmd->thrust.z; + + tf2::Quaternion q; + q.setRPY(cmd->roll, cmd->pitch, cmd->yaw_rate); + mavros_cmd.orientation.x = q.x(); + mavros_cmd.orientation.y = q.y(); + mavros_cmd.orientation.z = q.z(); + mavros_cmd.orientation.w = q.w(); + + attitude_target_pub_->publish(mavros_cmd); + } - mavros_msgs::msg::AttitudeTarget mavros_cmd; - mavros_cmd.header.stamp = this->get_clock()->now(); //.to_msg(); - mavros_cmd.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE | - mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE; - tf2::Matrix3x3 m; - m.setRPY(cmd->roll, cmd->pitch, yaw_); - tf2::Quaternion q; - m.getRotation(q); - mavros_cmd.body_rate.z = cmd->yaw_rate; - mavros_cmd.thrust = cmd->thrust.z; - - mavros_cmd.orientation.x = q.x(); - mavros_cmd.orientation.y = q.y(); - mavros_cmd.orientation.z = q.z(); - mavros_cmd.orientation.w = q.w(); - - attitude_target_pub_->publish(mavros_cmd); } void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr cmd) override { diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py index da3090672..958a405a1 100755 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py @@ -267,9 +267,9 @@ def get_line_waypoints(attributes): # length, height): for y in np.arange(0, -length, -0.5): wp = WaypointXYZVYaw() wp.position.x = -y - wp.position.y = 0 + wp.position.y = 0. wp.position.z = height - wp.yaw = 0 + wp.yaw = 0. traj.waypoints.append(wp) @@ -294,7 +294,7 @@ def get_point_waypoints(attributes): # length, height): wp.position.x = x wp.position.y = y wp.position.z = height - wp.yaw = 0 + wp.yaw = 0. traj.waypoints.append(wp) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/CMakeLists.txt new file mode 100644 index 000000000..9c5d71947 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(pid_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(mav_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pid_controller_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + + +add_executable(pid_controller src/pid_controller.cpp) +ament_target_dependencies(pid_controller + rclcpp + std_msgs + mav_msgs + nav_msgs + pid_controller_msgs + airstack_msgs + airstack_common + tf2 + tf2_ros) + +install(TARGETS + pid_controller + DESTINATION lib/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/package.xml new file mode 100644 index 000000000..75d601b2e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/package.xml @@ -0,0 +1,28 @@ + + + + pid_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + mav_msgs + nav_msgs + pid_controller_msgs + airstack_msgs + airstack_common + tf2 + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/src/pid_controller.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/src/pid_controller.cpp new file mode 100644 index 000000000..375cec6aa --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller/src/pid_controller.cpp @@ -0,0 +1,247 @@ +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class PID { +public: + + rclcpp::Node* node; + rclcpp::Time time_prev; + + pid_controller_msgs::msg::PIDInfo info; + + + rclcpp::Publisher::SharedPtr info_pub; + +public: + PID(rclcpp::Node* node, std::string name); + void set_target(double target); + double get_control(double measured, double ff_value=0.); + void reset_integrator(); +}; + +PID::PID(rclcpp::Node* node, std::string name) + : node(node) + , time_prev(0){ + airstack::dynamic_param(node, name + "_p", 1., &info.p); + airstack::dynamic_param(node, name + "_i", 0., &info.i); + airstack::dynamic_param(node, name + "_d", 0., &info.d); + airstack::dynamic_param(node, name + "_ff", 0., &info.ff); + + + airstack::dynamic_param(node, name + "_d_alpha", 0., &info.d_alpha); + + airstack::dynamic_param(node, name + "_min", -100000., &info.min); + airstack::dynamic_param(node, name + "_max", 100000., &info.max); + airstack::dynamic_param(node, name + "_constant", 0., &info.constant); + + info_pub = node->create_publisher(name + "_pid_info", 1); +} + +void PID::set_target(double target){ + info.target = target; +} + +double PID::get_control(double measured, double ff_value){ + info.measured = measured; + info.ff_value = ff_value; + + rclcpp::Time time_now = node->now(); + info.header.stamp = time_now; + if(time_prev.seconds() == 0){ + time_prev = time_now; + info_pub->publish(info); + return 0.; + } + info.dt = (time_now - time_prev).seconds(); + time_prev = time_now; + if(info.dt <= 0.){ + info_pub->publish(info); + return info.control; + } + + double error_prev = info.error; + info.error = info.target - info.measured; + info.p_component = info.p * info.error; + + info.integral += info.error * info.dt; + if(info.i == 0) + info.integral = 0; + info.i_component = info.i * info.integral; + if(info.i_component > info.max){ + info.integral = info.max / info.i; + info.i_component = info.i * info.integral; + } + else if(info.i_component < info.min){ + info.integral = info.min / info.i; + info.i_component = info.i * info.integral; + } + + info.derivative = info.d_alpha*info.derivative + (1. - info.d_alpha)*(info.error - error_prev)/info.dt; + info.d_component = info.d * info.derivative; + + info.ff_component = info.ff * info.ff_value; + + info.control = info.p_component + info.i_component + info.d_component + info.ff_component + info.constant; + info.control = std::max(info.min, std::min(info.max, info.control)); + + info_pub->publish(info); + return info.control; +} + +void PID::reset_integrator(){ + info.integral = 0.; +} + +class PIDControllerNode : public rclcpp::Node { +private: + // params + std::string target_frame; + double max_roll_pitch; + + PID x_pid, y_pid, z_pid, vx_pid, vy_pid, vz_pid; + + // variables + bool got_odometry; + nav_msgs::msg::Odometry odometry; + + // subscribers + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr odometry_sub; + rclcpp::Subscription::SharedPtr reset_integrators_sub; + tf2_ros::Buffer* tf_buffer; + tf2_ros::TransformListener* tf_listener; + + // publishers + rclcpp::Publisher::SharedPtr command_pub; + +public: + PIDControllerNode() + : Node("pid_controller") + , x_pid(this, "x") + , y_pid(this, "y") + , z_pid(this, "z") + , vx_pid(this, "vx") + , vy_pid(this, "vy") + , vz_pid(this, "vz"){ + // init params + target_frame = airstack::get_param(this, "target_frame", std::string("base_link")); + max_roll_pitch = airstack::get_param(this, "max_roll_pitch", 10.)*M_PI/180.; + + // init subscribers + odometry_sub = this->create_subscription("odometry", 1, + std::bind(&PIDControllerNode::odometry_callback, + this, std::placeholders::_1)); + tracking_point_sub = this->create_subscription("tracking_point", 1, + std::bind(&PIDControllerNode::tracking_point_callback, + this, std::placeholders::_1)); + reset_integrators_sub = this->create_subscription("reset_integrators", 1, + std::bind(&PIDControllerNode::reset_integrators_callback, + this, std::placeholders::_1)); + + tf_buffer = new tf2_ros::Buffer(this->get_clock()); + //tf_buffer->setUsingDedicatedThread(true); + tf_listener = new tf2_ros::TransformListener(*tf_buffer); + + + // init publishers + command_pub = this->create_publisher("command", 1); + + // init variables + got_odometry = false; + } + + void tracking_point_callback(const airstack_msgs::msg::Odometry::SharedPtr msg){ + if(!got_odometry) + return; + + // transform tracking point and odometry + airstack_msgs::msg::Odometry tp; + nav_msgs::msg::Odometry odom; + airstack_msgs::msg::Odometry temp = *msg; + bool tp_tf_success = tflib::transform_odometry(tf_buffer, temp, target_frame, target_frame, &tp, + rclcpp::Duration::from_seconds(0.1)); + if(!tp_tf_success){ + RCLCPP_ERROR_STREAM(get_logger(), "failed to transform tracking point"); + return; + } + bool odom_tf_success = tflib::transform_odometry(tf_buffer, odometry, target_frame, target_frame, &odom, + rclcpp::Duration::from_seconds(0.1)); + if(!odom_tf_success){ + RCLCPP_ERROR_STREAM(get_logger(), "failed to transform odometry"); + return; + } + + tf2::Vector3 tp_pos = tflib::to_tf(tp.pose.position); + tf2::Vector3 tp_vel = tflib::to_tf(tp.twist.linear); + tf2::Vector3 odom_pos = tflib::to_tf(odom.pose.pose.position); + tf2::Vector3 odom_vel = tflib::to_tf(odom.twist.twist.linear); + + x_pid.set_target(tp_pos.x()); + y_pid.set_target(tp_pos.y()); + z_pid.set_target(tp_pos.z()); + + double vx = x_pid.get_control(odom_pos.x()); + double vy = y_pid.get_control(odom_pos.y()); + double vz = z_pid.get_control(odom_pos.z()); + + vx_pid.set_target(vx); + vy_pid.set_target(vy); + vz_pid.set_target(vz); + + double roll = -vy_pid.get_control(odom_vel.y()); + double pitch = vx_pid.get_control(odom_vel.x()); + double thrust = vz_pid.get_control(odom_vel.z()); + + // compute control + mav_msgs::msg::RollPitchYawrateThrust command; + command.header.frame_id = target_frame; + command.header.stamp = tp.header.stamp; + + command.roll = roll;//-std::max(-max_roll_pitch, std::min(max_roll_pitch, tp.pose.position.y - odom.pose.pose.position.y)); + command.pitch = pitch;//std::max(-max_roll_pitch, std::min(max_roll_pitch, tp.pose.position.x - odom.pose.pose.position.x)); + + double _, yaw; + tf2::Matrix3x3(tflib::to_tf(msg->pose.orientation)).getRPY(_, _, yaw); + command.yaw_rate = yaw; + + //RCLCPP_INFO_STREAM(get_logger(), "roll pitch: " << (command.roll*180./M_PI) << " " << (command.pitch*180./M_PI)); + //RCLCPP_INFO_STREAM(get_logger(), "min max: " << vx_pid.info.min << " " << vx_pid.info.max << " " << vy_pid.info.min << " " << vy_pid.info.max); + + command.thrust.z = thrust;//0.5; + + command_pub->publish(command); + } + + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg){ + got_odometry = true; + odometry = *msg; + } + + void reset_integrators_callback(const std_msgs::msg::Empty::SharedPtr msg){ + RCLCPP_INFO_STREAM(get_logger(), "RESET INTEGRATORS"); + x_pid.reset_integrator(); + y_pid.reset_integrator(); + z_pid.reset_integrator(); + vx_pid.reset_integrator(); + vy_pid.reset_integrator(); + vz_pid.reset_integrator(); + } + +}; + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/CMakeLists.txt new file mode 100644 index 000000000..415fdcfb9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(pid_controller_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/PIDInfo.msg" + DEPENDENCIES std_msgs + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/msg/PIDInfo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/msg/PIDInfo.msg new file mode 100644 index 000000000..de1284422 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/msg/PIDInfo.msg @@ -0,0 +1,28 @@ +std_msgs/Header header + +float64 p +float64 i +float64 d +float64 ff + +float64 d_alpha + +float64 min +float64 max + +float64 dt + +float64 error +float64 integral +float64 derivative +float64 ff_value + +float64 p_component +float64 i_component +float64 d_component +float64 ff_component +float64 constant +float64 control + +float64 target +float64 measured \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/package.xml new file mode 100644 index 000000000..8dd8872f5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/pid_controller_msgs/package.xml @@ -0,0 +1,25 @@ + + + + pid_controller_msgs + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml index 2bf167318..3ae07513f 100644 --- a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -82,6 +82,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ?> +